feat(zed): recover corrupted frame gaps in MCAP export

Make ZED MCAP export skip corrupted frame runs until recovery and

treat unreadable tail frames as end-of-stream instead of hard

failing conversion.

Update bundled nearest-mode export to emit partial bundles during

corruption gaps, extend BundleManifest with explicit member status

and skipped-frame counts, and only write payload messages for

present cameras.

Tighten batch probing so bundled MCAP validation checks /bundle

coverage and per-camera message counts, and improve failure

excerpts to include stderr tail output.

Also add a local cppzmq CMake fallback, refresh the multi-record

tester for the new bundle schema, and document the mixed NVENC

limitations in the README.
This commit is contained in:
2026-03-24 03:49:35 +00:00
parent e3a423433e
commit 807a73b480
8 changed files with 1006 additions and 172 deletions
+12 -2
View File
@@ -28,6 +28,14 @@ if (NOT TARGET OpenSSL::Crypto AND DEFINED OPENSSL_CRYPTO_LIBRARY)
INTERFACE_INCLUDE_DIRECTORIES "${OPENSSL_INCLUDE_DIR}") INTERFACE_INCLUDE_DIRECTORIES "${OPENSSL_INCLUDE_DIR}")
endif() endif()
find_package(cppzmq QUIET) find_package(cppzmq QUIET)
set(CPPZMQ_LOCAL_ROOT "${CMAKE_CURRENT_LIST_DIR}/../cppzmq" CACHE PATH "Path to a local cppzmq checkout")
if (NOT TARGET cppzmq::cppzmq AND NOT TARGET cppzmq)
if (EXISTS "${CPPZMQ_LOCAL_ROOT}/zmq.hpp")
add_library(cppzmq::cppzmq INTERFACE IMPORTED GLOBAL)
set_target_properties(cppzmq::cppzmq PROPERTIES
INTERFACE_INCLUDE_DIRECTORIES "${CPPZMQ_LOCAL_ROOT}")
endif()
endif()
if (DEFINED CVMMAP_STREAMER_USE_SYSTEM_CNATS) if (DEFINED CVMMAP_STREAMER_USE_SYSTEM_CNATS)
message(FATAL_ERROR message(FATAL_ERROR
"CVMMAP_STREAMER_USE_SYSTEM_CNATS was removed; use CVMMAP_CNATS_PROVIDER=system") "CVMMAP_STREAMER_USE_SYSTEM_CNATS was removed; use CVMMAP_CNATS_PROVIDER=system")
@@ -161,8 +169,10 @@ protobuf_generate(
TARGET cvmmap_streamer_depth_proto TARGET cvmmap_streamer_depth_proto
LANGUAGE cpp LANGUAGE cpp
PROTOC_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}" PROTOC_OUT_DIR "${CMAKE_CURRENT_BINARY_DIR}"
PROTOS "${CMAKE_CURRENT_LIST_DIR}/proto/cvmmap_streamer/DepthMap.proto" PROTOS
IMPORT_DIRS "${CMAKE_CURRENT_LIST_DIR}") "${CMAKE_CURRENT_LIST_DIR}/proto/cvmmap_streamer/DepthMap.proto"
"${CMAKE_CURRENT_LIST_DIR}/proto/cvmmap_streamer/BundleManifest.proto"
IMPORT_DIRS "${CMAKE_CURRENT_LIST_DIR}/proto")
add_library(cvmmap_streamer_protobuf INTERFACE) add_library(cvmmap_streamer_protobuf INTERFACE)
target_include_directories(cvmmap_streamer_foxglove_proto target_include_directories(cvmmap_streamer_foxglove_proto
PUBLIC PUBLIC
+3 -1
View File
@@ -265,9 +265,11 @@ uv run python scripts/zed_batch_svo_to_mcap.py \
The batch MCAP wrapper writes `<segment>/<segment>.mcap` by default, skips existing outputs unless told otherwise, and returns a nonzero exit code if any segment fails. The batch MCAP wrapper writes `<segment>/<segment>.mcap` by default, skips existing outputs unless told otherwise, and returns a nonzero exit code if any segment fails.
The repo includes a minimal pose config at `config/zed_pose_config.toml` so MCAP conversion does not depend on a separate `cv-mmap` checkout. The repo includes a minimal pose config at `config/zed_pose_config.toml` so MCAP conversion does not depend on a separate `cv-mmap` checkout.
In bundled multi-camera mode, `--start-frame` and `--end-frame` mean the first and last emitted synced frame-group indices from the common start timestamp, inclusive. In bundled multi-camera mode, `--start-frame` and `--end-frame` mean the first and last emitted bundle indices from the common start timestamp, inclusive.
When stderr is attached to a TTY, `zed_batch_svo_to_mcap.py` uses a `progress-table` view by default; otherwise it emits line-oriented start/completion/failure logs plus periodic heartbeat summaries. Use `--progress-ui table` or `--progress-ui text` to override the automatic mode selection. When stderr is attached to a TTY, `zed_batch_svo_to_mcap.py` uses a `progress-table` view by default; otherwise it emits line-oriented start/completion/failure logs plus periodic heartbeat summaries. Use `--progress-ui table` or `--progress-ui text` to override the automatic mode selection.
Bundled MCAP export now defaults to `--bundle-policy nearest`. That mode emits one `/bundle` manifest message per bundle timestamp on the common timeline and keeps the original per-camera timestamps on `/zedN/video`, `/zedN/depth`, and optional `/zedN/pose`. Consumers that care about grouping should follow `/bundle` instead of inferring bundle membership from identical message timestamps. Use `--bundle-policy strict` when you want the older thresholded sync behavior, and `--sync-tolerance-ms` only applies in that strict mode.
### Why Mixed Hardware/Software Mode Exists ### Why Mixed Hardware/Software Mode Exists
Bundled MCAP export opens one video encoder per camera stream. A four-camera segment therefore consumes four H.264/H.265 encoder sessions at once. Bundled MCAP export opens one video encoder per camera stream. A four-camera segment therefore consumes four H.264/H.265 encoder sessions at once.
@@ -8,6 +8,7 @@
#include <cstddef> #include <cstddef>
#include <cstdint> #include <cstdint>
#include <expected> #include <expected>
#include <optional>
#include <span> #include <span>
#include <string> #include <string>
#include <string_view> #include <string_view>
@@ -19,6 +20,16 @@ enum class DepthEncoding {
RvlF32, RvlF32,
}; };
enum class BundlePolicy {
Nearest,
Strict,
};
enum class BundleMemberStatus {
Present,
CorruptedGap,
};
struct RawDepthMapView { struct RawDepthMapView {
std::uint64_t timestamp_ns{0}; std::uint64_t timestamp_ns{0};
std::uint32_t width{0}; std::uint32_t width{0};
@@ -57,6 +68,21 @@ struct RawBodyTrackingMessageView {
std::span<const std::uint8_t> bytes{}; std::span<const std::uint8_t> bytes{};
}; };
struct RawBundleMemberView {
std::string_view camera_label{};
BundleMemberStatus status{BundleMemberStatus::Present};
std::optional<std::uint64_t> timestamp_ns{};
std::int64_t delta_ns{0};
std::uint32_t corrupted_frames_skipped{0};
};
struct RawBundleManifestView {
std::uint64_t timestamp_ns{0};
std::uint64_t bundle_index{0};
BundlePolicy policy{BundlePolicy::Nearest};
std::span<const RawBundleMemberView> members{};
};
struct McapRecordStreamConfig { struct McapRecordStreamConfig {
std::string topic{"/camera/video"}; std::string topic{"/camera/video"};
std::string depth_topic{"/camera/depth"}; std::string depth_topic{"/camera/depth"};
@@ -137,7 +163,8 @@ public:
[[nodiscard]] [[nodiscard]]
static std::expected<MultiMcapRecordSink, std::string> create( static std::expected<MultiMcapRecordSink, std::string> create(
std::string path, std::string path,
McapCompression compression); McapCompression compression,
std::string bundle_topic = "/bundle");
[[nodiscard]] [[nodiscard]]
std::expected<StreamId, std::string> add_stream( std::expected<StreamId, std::string> add_stream(
@@ -179,6 +206,10 @@ public:
StreamId stream_id, StreamId stream_id,
const RawPoseView &pose); const RawPoseView &pose);
[[nodiscard]]
std::expected<void, std::string> write_bundle_manifest(
const RawBundleManifestView &bundle);
[[nodiscard]] [[nodiscard]]
std::expected<void, std::string> write_body_tracking_message( std::expected<void, std::string> write_body_tracking_message(
StreamId stream_id, StreamId stream_id,
@@ -0,0 +1,32 @@
syntax = "proto3";
package cvmmap_streamer;
import "google/protobuf/timestamp.proto";
message BundleManifest {
enum BundlePolicy {
BUNDLE_POLICY_UNKNOWN = 0;
BUNDLE_POLICY_NEAREST = 1;
BUNDLE_POLICY_STRICT = 2;
}
enum BundleMemberStatus {
BUNDLE_MEMBER_STATUS_UNKNOWN = 0;
BUNDLE_MEMBER_STATUS_PRESENT = 1;
BUNDLE_MEMBER_STATUS_CORRUPTED_GAP = 2;
}
message BundleMember {
string camera_label = 1;
google.protobuf.Timestamp timestamp = 2;
sint64 delta_ns = 3;
BundleMemberStatus status = 4;
uint32 corrupted_frames_skipped = 5;
}
google.protobuf.Timestamp timestamp = 1;
uint64 bundle_index = 2;
BundlePolicy policy = 3;
repeated BundleMember members = 4;
}
+171 -11
View File
@@ -10,6 +10,7 @@ import re
import subprocess import subprocess
import sys import sys
import time import time
from collections import Counter
from dataclasses import dataclass from dataclasses import dataclass
from pathlib import Path from pathlib import Path
@@ -34,6 +35,8 @@ class BatchConfig:
mcap_compression: str mcap_compression: str
depth_mode: str depth_mode: str
depth_size: str depth_size: str
bundle_policy: str
bundle_topic: str | None
with_pose: bool with_pose: bool
pose_config: Path | None pose_config: Path | None
world_frame_id: str | None world_frame_id: str | None
@@ -102,6 +105,7 @@ class ActiveJobState:
_MCAP_READER_MODULE = None _MCAP_READER_MODULE = None
_BUNDLE_MANIFEST_CLASS_CACHE: dict[bytes, tuple[object, int | None]] = {}
TABLE_REFRESH_SECONDS = 1.0 TABLE_REFRESH_SECONDS = 1.0
TEXT_HEARTBEAT_SECONDS = 30.0 TEXT_HEARTBEAT_SECONDS = 30.0
@@ -500,7 +504,11 @@ def command_for_job(job: ConversionJob, config: BatchConfig, encoder_device: str
config.depth_mode, config.depth_mode,
"--depth-size", "--depth-size",
config.depth_size, config.depth_size,
"--bundle-policy",
config.bundle_policy,
] ]
if config.bundle_topic:
command.extend(["--bundle-topic", config.bundle_topic])
if config.with_pose: if config.with_pose:
command.append("--with-pose") command.append("--with-pose")
if config.pose_config is not None: if config.pose_config is not None:
@@ -569,22 +577,131 @@ def required_topics_for(camera_labels: tuple[str, ...]) -> set[str]:
return topics return topics
def probe_output(output_path: Path, camera_labels: tuple[str, ...]) -> OutputProbeResult: def load_bundle_manifest_type(schema_data: bytes) -> tuple[object, int | None]:
cached = _BUNDLE_MANIFEST_CLASS_CACHE.get(schema_data)
if cached is not None:
return cached
from google.protobuf import descriptor_pb2, descriptor_pool, message_factory, timestamp_pb2
descriptor_set = descriptor_pb2.FileDescriptorSet()
descriptor_set.ParseFromString(schema_data)
pool = descriptor_pool.DescriptorPool()
has_embedded_timestamp = any(
file_descriptor.name == "google/protobuf/timestamp.proto"
for file_descriptor in descriptor_set.file
)
if has_embedded_timestamp:
for file_descriptor in descriptor_set.file:
if file_descriptor.name == "google/protobuf/timestamp.proto":
pool.Add(file_descriptor)
break
else:
pool.AddSerializedFile(timestamp_pb2.DESCRIPTOR.serialized_pb)
for file_descriptor in descriptor_set.file:
if file_descriptor.name == "google/protobuf/timestamp.proto":
continue
pool.Add(file_descriptor)
message_descriptor = pool.FindMessageTypeByName("cvmmap_streamer.BundleManifest")
message_class = message_factory.GetMessageClass(message_descriptor)
present_value = None
if "BundleMemberStatus" in message_descriptor.enum_types_by_name:
status_enum = message_descriptor.enum_types_by_name["BundleMemberStatus"]
present_value = status_enum.values_by_name["BUNDLE_MEMBER_STATUS_PRESENT"].number
_BUNDLE_MANIFEST_CLASS_CACHE[schema_data] = (message_class, present_value)
return message_class, present_value
def probe_output(
output_path: Path,
camera_labels: tuple[str, ...],
*,
bundle_topic: str | None,
) -> OutputProbeResult:
if not output_path.is_file(): if not output_path.is_file():
return OutputProbeResult(output_path=output_path, status="missing") return OutputProbeResult(output_path=output_path, status="missing")
reader_module = load_mcap_reader() reader_module = load_mcap_reader()
expected_topics = required_topics_for(camera_labels) expected_topics = required_topics_for(camera_labels)
require_bundle = len(camera_labels) > 1 and bool(bundle_topic)
if require_bundle:
expected_topics.add(bundle_topic or "/bundle")
found_topics: set[str] = set() found_topics: set[str] = set()
video_counts: Counter[str] = Counter()
depth_counts: Counter[str] = Counter()
bundle_present_counts: Counter[str] = Counter()
expected_camera_labels = set(camera_labels)
try: try:
with output_path.open("rb") as stream: with output_path.open("rb") as stream:
reader = reader_module.make_reader(stream) reader = reader_module.make_reader(stream)
for _schema, channel, _message in reader.iter_messages(): for schema, channel, message in reader.iter_messages():
if channel.topic in expected_topics: if channel.topic in expected_topics:
found_topics.add(channel.topic) found_topics.add(channel.topic)
if found_topics == expected_topics: if channel.topic.endswith("/video"):
return OutputProbeResult(output_path=output_path, status="valid") video_counts[channel.topic.removeprefix("/").removesuffix("/video")] += 1
continue
if channel.topic.endswith("/depth"):
depth_counts[channel.topic.removeprefix("/").removesuffix("/depth")] += 1
continue
if require_bundle and channel.topic == bundle_topic:
if schema is None or schema.name != "cvmmap_streamer.BundleManifest":
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=f"bundle topic '{bundle_topic}' is missing the BundleManifest schema",
)
try:
bundle_class, present_value = load_bundle_manifest_type(schema.data)
bundle = bundle_class()
bundle.ParseFromString(message.data)
except Exception as error: # noqa: BLE001
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=f"failed to parse bundle manifest: {error}",
)
bundle_labels: set[str] = set()
for member in bundle.members:
label = str(member.camera_label)
if label not in expected_camera_labels:
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=f"bundle manifest referenced unknown camera label '{label}'",
)
if label in bundle_labels:
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=f"bundle manifest duplicated camera label '{label}'",
)
bundle_labels.add(label)
is_present = member.HasField("timestamp")
if present_value is not None:
is_present = member.status == present_value
if is_present and not member.HasField("timestamp"):
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=f"bundle member '{label}' is present but missing a timestamp",
)
if is_present:
bundle_present_counts[label] += 1
if bundle_labels != expected_camera_labels:
missing_labels = sorted(expected_camera_labels - bundle_labels)
extra_labels = sorted(bundle_labels - expected_camera_labels)
details = []
if missing_labels:
details.append("missing=" + ",".join(missing_labels))
if extra_labels:
details.append("extra=" + ",".join(extra_labels))
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason="bundle manifest camera coverage mismatch: " + " ".join(details),
)
except Exception as error: # noqa: BLE001 except Exception as error: # noqa: BLE001
return OutputProbeResult(output_path=output_path, status="invalid", reason=str(error)) return OutputProbeResult(output_path=output_path, status="invalid", reason=str(error))
@@ -595,6 +712,27 @@ def probe_output(output_path: Path, camera_labels: tuple[str, ...]) -> OutputPro
status="invalid", status="invalid",
reason="missing expected topics: " + ", ".join(missing_topics), reason="missing expected topics: " + ", ".join(missing_topics),
) )
if require_bundle:
for label in camera_labels:
present_count = bundle_present_counts[label]
if video_counts[label] != present_count:
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=(
f"video count mismatch for {label}: "
f"bundle_present={present_count} video_messages={video_counts[label]}"
),
)
if depth_counts[label] != present_count:
return OutputProbeResult(
output_path=output_path,
status="invalid",
reason=(
f"depth count mismatch for {label}: "
f"bundle_present={present_count} depth_messages={depth_counts[label]}"
),
)
return OutputProbeResult(output_path=output_path, status="valid") return OutputProbeResult(output_path=output_path, status="valid")
@@ -635,8 +773,13 @@ def split_lines_for_excerpt(text: str, max_lines: int = 8) -> list[str]:
lines = [line.rstrip() for line in text.splitlines() if line.strip()] lines = [line.rstrip() for line in text.splitlines() if line.strip()]
if len(lines) <= max_lines: if len(lines) <= max_lines:
return lines return lines
excerpt = lines[:max_lines] head_count = max(1, max_lines // 2)
excerpt.append(f"... ({len(lines) - max_lines} more lines)") tail_count = max_lines - head_count
excerpt = lines[:head_count]
omitted = len(lines) - head_count - tail_count
if omitted > 0:
excerpt.append(f"... ({omitted} omitted line(s))")
excerpt.extend(lines[-tail_count:])
return excerpt return excerpt
@@ -1011,6 +1154,19 @@ def build_worker_slots(
default="optimal", default="optimal",
show_default=True, show_default=True,
) )
@click.option(
"--bundle-policy",
type=click.Choice(("nearest", "strict")),
default="nearest",
show_default=True,
help="Bundling policy for multi-camera MCAP export.",
)
@click.option(
"--bundle-topic",
default="/bundle",
show_default=True,
help="Topic used for bundled multi-camera manifest messages.",
)
@click.option("--with-pose", is_flag=True, help="Enable per-camera positional tracking export when available.") @click.option("--with-pose", is_flag=True, help="Enable per-camera positional tracking export when available.")
@click.option( @click.option(
"--pose-config", "--pose-config",
@@ -1026,19 +1182,19 @@ def build_worker_slots(
"--start-frame", "--start-frame",
type=click.IntRange(min=0), type=click.IntRange(min=0),
default=None, default=None,
help="First synced frame group to export (inclusive) in bundled multi-camera mode.", help="First bundle index to export (inclusive) in bundled multi-camera mode.",
) )
@click.option( @click.option(
"--end-frame", "--end-frame",
type=click.IntRange(min=0), type=click.IntRange(min=0),
default=None, default=None,
help="Last synced frame group to export (inclusive) in bundled multi-camera mode.", help="Last bundle index to export (inclusive) in bundled multi-camera mode.",
) )
@click.option( @click.option(
"--sync-tolerance-ms", "--sync-tolerance-ms",
type=click.FloatRange(min=0.0, min_open=True), type=click.FloatRange(min=0.0, min_open=True),
default=None, default=None,
help="Override the maximum timestamp delta used for bundled multi-camera sync.", help="Override the maximum timestamp delta used by strict bundled sync.",
) )
@click.option( @click.option(
"--progress-ui", "--progress-ui",
@@ -1070,6 +1226,8 @@ def main(
mcap_compression: str, mcap_compression: str,
depth_mode: str, depth_mode: str,
depth_size: str, depth_size: str,
bundle_policy: str,
bundle_topic: str,
with_pose: bool, with_pose: bool,
pose_config: Path | None, pose_config: Path | None,
world_frame_id: str | None, world_frame_id: str | None,
@@ -1102,6 +1260,8 @@ def main(
mcap_compression=mcap_compression, mcap_compression=mcap_compression,
depth_mode=depth_mode, depth_mode=depth_mode,
depth_size=depth_size, depth_size=depth_size,
bundle_policy=bundle_policy,
bundle_topic=bundle_topic,
with_pose=with_pose, with_pose=with_pose,
pose_config=pose_config.expanduser().resolve() if pose_config is not None else None, pose_config=pose_config.expanduser().resolve() if pose_config is not None else None,
world_frame_id=world_frame_id, world_frame_id=world_frame_id,
@@ -1156,7 +1316,7 @@ def main(
continue continue
if report_existing: if report_existing:
probe_result = probe_output(output_path, job.camera_labels) probe_result = probe_output(output_path, job.camera_labels, bundle_topic=config.bundle_topic)
if probe_result.status == "valid": if probe_result.status == "valid":
valid_existing.append(probe_result) valid_existing.append(probe_result)
elif probe_result.status == "invalid": elif probe_result.status == "invalid":
@@ -1171,7 +1331,7 @@ def main(
continue continue
if config.probe_existing: if config.probe_existing:
probe_result = probe_output(output_path, job.camera_labels) probe_result = probe_output(output_path, job.camera_labels, bundle_topic=config.bundle_topic)
if probe_result.status == "valid": if probe_result.status == "valid":
valid_existing.append(probe_result) valid_existing.append(probe_result)
skipped_results.append( skipped_results.append(
+111 -1
View File
@@ -3,6 +3,7 @@
#include "cvmmap_streamer/record/mcap_record_sink.hpp" #include "cvmmap_streamer/record/mcap_record_sink.hpp"
#include "protobuf_descriptor.hpp" #include "protobuf_descriptor.hpp"
#include "proto/cvmmap_streamer/BundleManifest.pb.h"
#include "proto/cvmmap_streamer/DepthMap.pb.h" #include "proto/cvmmap_streamer/DepthMap.pb.h"
#include "proto/foxglove/CameraCalibration.pb.h" #include "proto/foxglove/CameraCalibration.pb.h"
#include "proto/foxglove/CompressedVideo.pb.h" #include "proto/foxglove/CompressedVideo.pb.h"
@@ -117,6 +118,28 @@ cvmmap_streamer::DepthMap::Encoding to_proto_depth_encoding(DepthEncoding encodi
} }
} }
[[nodiscard]]
cvmmap_streamer::BundleManifest::BundlePolicy to_proto_bundle_policy(BundlePolicy policy) {
switch (policy) {
case BundlePolicy::Strict:
return cvmmap_streamer::BundleManifest::BUNDLE_POLICY_STRICT;
case BundlePolicy::Nearest:
default:
return cvmmap_streamer::BundleManifest::BUNDLE_POLICY_NEAREST;
}
}
[[nodiscard]]
cvmmap_streamer::BundleManifest::BundleMemberStatus to_proto_bundle_member_status(BundleMemberStatus status) {
switch (status) {
case BundleMemberStatus::CorruptedGap:
return cvmmap_streamer::BundleManifest::BUNDLE_MEMBER_STATUS_CORRUPTED_GAP;
case BundleMemberStatus::Present:
default:
return cvmmap_streamer::BundleManifest::BUNDLE_MEMBER_STATUS_PRESENT;
}
}
void append_start_code(std::vector<std::uint8_t> &output) { void append_start_code(std::vector<std::uint8_t> &output) {
output.push_back(0x00); output.push_back(0x00);
output.push_back(0x00); output.push_back(0x00);
@@ -464,6 +487,60 @@ std::expected<void, std::string> write_calibration_message(
return {}; return {};
} }
[[nodiscard]]
std::expected<void, std::string> write_bundle_manifest_message(
mcap::McapWriter &writer,
mcap::ChannelId channel_id,
std::uint32_t &sequence,
const RawBundleManifestView &bundle) {
if (channel_id == 0) {
return std::unexpected("bundle topic is disabled");
}
if (bundle.members.empty()) {
return std::unexpected("bundle manifest must contain at least one member");
}
cvmmap_streamer::BundleManifest message{};
*message.mutable_timestamp() = to_proto_timestamp(bundle.timestamp_ns);
message.set_bundle_index(bundle.bundle_index);
message.set_policy(to_proto_bundle_policy(bundle.policy));
for (const auto &member_view : bundle.members) {
if (member_view.camera_label.empty()) {
return std::unexpected("bundle member camera label is empty");
}
if (member_view.status == BundleMemberStatus::Present && !member_view.timestamp_ns.has_value()) {
return std::unexpected("present bundle member is missing a timestamp");
}
auto *member = message.add_members();
member->set_camera_label(std::string(member_view.camera_label));
member->set_status(to_proto_bundle_member_status(member_view.status));
if (member_view.timestamp_ns.has_value()) {
*member->mutable_timestamp() = to_proto_timestamp(*member_view.timestamp_ns);
}
member->set_delta_ns(member_view.delta_ns);
member->set_corrupted_frames_skipped(member_view.corrupted_frames_skipped);
}
std::string serialized{};
if (!message.SerializeToString(&serialized)) {
return std::unexpected("failed to serialize cvmmap_streamer.BundleManifest");
}
mcap::Message record{};
record.channelId = channel_id;
record.sequence = sequence++;
record.logTime = bundle.timestamp_ns;
record.publishTime = bundle.timestamp_ns;
record.data = reinterpret_cast<const std::byte *>(serialized.data());
record.dataSize = serialized.size();
const auto write_status = writer.write(record);
if (!write_status.ok()) {
return std::unexpected("failed to write MCAP bundle manifest: " + write_status.message);
}
return {};
}
} }
struct McapRecordSink::State { struct McapRecordSink::State {
@@ -835,8 +912,12 @@ struct MultiMcapRecordSink::State {
std::string path{}; std::string path{};
mcap::SchemaId video_schema_id{0}; mcap::SchemaId video_schema_id{0};
mcap::SchemaId depth_schema_id{0}; mcap::SchemaId depth_schema_id{0};
mcap::SchemaId bundle_schema_id{0};
mcap::SchemaId calibration_schema_id{0}; mcap::SchemaId calibration_schema_id{0};
mcap::SchemaId pose_schema_id{0}; mcap::SchemaId pose_schema_id{0};
std::string bundle_topic{};
mcap::ChannelId bundle_channel_id{0};
std::uint32_t bundle_sequence{0};
std::vector<StreamState> streams{}; std::vector<StreamState> streams{};
}; };
@@ -959,10 +1040,12 @@ MultiMcapRecordSink &MultiMcapRecordSink::operator=(MultiMcapRecordSink &&other)
std::expected<MultiMcapRecordSink, std::string> MultiMcapRecordSink::create( std::expected<MultiMcapRecordSink, std::string> MultiMcapRecordSink::create(
std::string path, std::string path,
McapCompression compression) { McapCompression compression,
std::string bundle_topic) {
MultiMcapRecordSink sink{}; MultiMcapRecordSink sink{};
auto state = std::make_unique<State>(); auto state = std::make_unique<State>();
state->path = std::move(path); state->path = std::move(path);
state->bundle_topic = std::move(bundle_topic);
mcap::McapWriterOptions options(""); mcap::McapWriterOptions options("");
options.compression = to_mcap_compression(compression); options.compression = to_mcap_compression(compression);
@@ -989,6 +1072,21 @@ std::expected<MultiMcapRecordSink, std::string> MultiMcapRecordSink::create(
state->writer.addSchema(depth_schema); state->writer.addSchema(depth_schema);
state->depth_schema_id = depth_schema.id; state->depth_schema_id = depth_schema.id;
if (!state->bundle_topic.empty()) {
const auto bundle_descriptor_set = build_file_descriptor_set(cvmmap_streamer::BundleManifest::descriptor());
std::string bundle_schema_bytes{};
if (!bundle_descriptor_set.SerializeToString(&bundle_schema_bytes)) {
return std::unexpected("failed to serialize cvmmap_streamer.BundleManifest descriptor set");
}
mcap::Schema bundle_schema("cvmmap_streamer.BundleManifest", "protobuf", bundle_schema_bytes);
state->writer.addSchema(bundle_schema);
state->bundle_schema_id = bundle_schema.id;
mcap::Channel bundle_channel(state->bundle_topic, "protobuf", state->bundle_schema_id);
state->writer.addChannel(bundle_channel);
state->bundle_channel_id = bundle_channel.id;
}
const auto calibration_descriptor_set = build_file_descriptor_set(foxglove::CameraCalibration::descriptor()); const auto calibration_descriptor_set = build_file_descriptor_set(foxglove::CameraCalibration::descriptor());
std::string calibration_schema_bytes{}; std::string calibration_schema_bytes{};
if (!calibration_descriptor_set.SerializeToString(&calibration_schema_bytes)) { if (!calibration_descriptor_set.SerializeToString(&calibration_schema_bytes)) {
@@ -1237,6 +1335,18 @@ std::expected<void, std::string> MultiMcapRecordSink::write_pose(
return {}; return {};
} }
std::expected<void, std::string> MultiMcapRecordSink::write_bundle_manifest(
const RawBundleManifestView &bundle) {
if (state_ == nullptr) {
return std::unexpected("MCAP sink is not open");
}
return write_bundle_manifest_message(
state_->writer,
state_->bundle_channel_id,
state_->bundle_sequence,
bundle);
}
std::expected<void, std::string> MultiMcapRecordSink::write_body_tracking_message( std::expected<void, std::string> MultiMcapRecordSink::write_body_tracking_message(
const StreamId stream_id, const StreamId stream_id,
const RawBodyTrackingMessageView &body_message) { const RawBodyTrackingMessageView &body_message) {
+53 -1
View File
@@ -1,5 +1,6 @@
#include <mcap/reader.hpp> #include <mcap/reader.hpp>
#include "proto/cvmmap_streamer/BundleManifest.pb.h"
#include "proto/cvmmap_streamer/DepthMap.pb.h" #include "proto/cvmmap_streamer/DepthMap.pb.h"
#include "cvmmap_streamer/common.h" #include "cvmmap_streamer/common.h"
#include "cvmmap_streamer/record/mcap_record_sink.hpp" #include "cvmmap_streamer/record/mcap_record_sink.hpp"
@@ -73,7 +74,8 @@ int main(int argc, char **argv) {
auto sink = cvmmap_streamer::record::MultiMcapRecordSink::create( auto sink = cvmmap_streamer::record::MultiMcapRecordSink::create(
output_path.string(), output_path.string(),
compression); compression,
"/bundle");
if (!sink) { if (!sink) {
spdlog::error("failed to create MCAP sink: {}", sink.error()); spdlog::error("failed to create MCAP sink: {}", sink.error());
return exit_code(TesterExitCode::CreateError); return exit_code(TesterExitCode::CreateError);
@@ -125,6 +127,31 @@ int main(int argc, char **argv) {
0.0, 500.0, 240.0, 0.0, 0.0, 500.0, 240.0, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0,
}; };
const std::vector<cvmmap_streamer::record::RawBundleMemberView> bundle_members{
{
.camera_label = "zed1",
.status = cvmmap_streamer::record::BundleMemberStatus::Present,
.timestamp_ns = 100,
.delta_ns = -5,
},
{
.camera_label = "zed2",
.status = cvmmap_streamer::record::BundleMemberStatus::CorruptedGap,
.timestamp_ns = std::nullopt,
.delta_ns = 0,
.corrupted_frames_skipped = 3,
},
};
if (auto write = sink->write_bundle_manifest(cvmmap_streamer::record::RawBundleManifestView{
.timestamp_ns = 105,
.bundle_index = 7,
.policy = cvmmap_streamer::record::BundlePolicy::Nearest,
.members = bundle_members,
}); !write) {
spdlog::error("failed to write bundle manifest: {}", write.error());
return exit_code(TesterExitCode::WriteError);
}
for (const auto [stream_id, label, pose_x] : { for (const auto [stream_id, label, pose_x] : {
std::tuple{*zed1, std::string("zed1"), 1.0}, std::tuple{*zed1, std::string("zed1"), 1.0},
@@ -242,6 +269,30 @@ int main(int argc, char **argv) {
continue; continue;
} }
if (it->schema->name == "cvmmap_streamer.BundleManifest") {
cvmmap_streamer::BundleManifest bundle{};
if (!bundle.ParseFromArray(it->message.data, static_cast<int>(it->message.dataSize))) {
spdlog::error("failed to parse cvmmap_streamer.BundleManifest");
reader.close();
return exit_code(TesterExitCode::VerificationError);
}
if (bundle.bundle_index() != 7 || bundle.members_size() != 2) {
spdlog::error("bundle manifest contents mismatch");
reader.close();
return exit_code(TesterExitCode::VerificationError);
}
if (bundle.members(0).status() != cvmmap_streamer::BundleManifest::BUNDLE_MEMBER_STATUS_PRESENT ||
!bundle.members(0).has_timestamp() ||
bundle.members(1).status() != cvmmap_streamer::BundleManifest::BUNDLE_MEMBER_STATUS_CORRUPTED_GAP ||
bundle.members(1).has_timestamp() ||
bundle.members(1).corrupted_frames_skipped() != 3) {
spdlog::error("bundle manifest member status mismatch");
reader.close();
return exit_code(TesterExitCode::VerificationError);
}
continue;
}
if (it->schema->name == "foxglove.CameraCalibration") { if (it->schema->name == "foxglove.CameraCalibration") {
foxglove::CameraCalibration calibration{}; foxglove::CameraCalibration calibration{};
if (!calibration.ParseFromArray(it->message.data, static_cast<int>(it->message.dataSize))) { if (!calibration.ParseFromArray(it->message.data, static_cast<int>(it->message.dataSize))) {
@@ -266,6 +317,7 @@ int main(int argc, char **argv) {
reader.close(); reader.close();
for (const auto &topic : { for (const auto &topic : {
"/bundle",
"/zed1/video", "/zed1/video",
"/zed1/depth", "/zed1/depth",
"/zed1/calibration", "/zed1/calibration",
File diff suppressed because it is too large Load Diff