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:
+12
-2
@@ -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
|
||||||
|
|||||||
@@ -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;
|
||||||
|
}
|
||||||
@@ -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(
|
||||||
|
|||||||
@@ -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) {
|
||||||
|
|||||||
@@ -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",
|
||||||
|
|||||||
+592
-155
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user