feat(record): add depth RVL recording to MCAP
This commit is contained in:
@@ -192,6 +192,44 @@ bool frame_info_equal(const ipc::FrameInfo &lhs, const ipc::FrameInfo &rhs) {
|
||||
lhs.buffer_size == rhs.buffer_size;
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
std::expected<record::RawDepthMapView, std::string> make_depth_map_view(const ipc::CoherentSnapshot &snapshot) {
|
||||
if (!snapshot.depth_info) {
|
||||
return std::unexpected("depth plane metadata is missing");
|
||||
}
|
||||
if (snapshot.depth.empty()) {
|
||||
return std::unexpected("depth plane bytes are missing");
|
||||
}
|
||||
if (snapshot.depth_unit == ipc::DepthUnit::Unknown) {
|
||||
return std::unexpected("depth plane unit is unknown");
|
||||
}
|
||||
|
||||
const auto &depth_info = *snapshot.depth_info;
|
||||
if (depth_info.depth != ipc::Depth::F32 || depth_info.pixel_format != ipc::PixelFormat::GRAY) {
|
||||
return std::unexpected("depth plane must be GRAY/F32");
|
||||
}
|
||||
|
||||
const auto pixel_count = static_cast<std::size_t>(depth_info.width) * static_cast<std::size_t>(depth_info.height);
|
||||
const auto expected_bytes = pixel_count * sizeof(float);
|
||||
if (snapshot.depth.size() != expected_bytes) {
|
||||
return std::unexpected(
|
||||
"depth plane byte size does not match width*height*sizeof(float)");
|
||||
}
|
||||
if ((reinterpret_cast<std::uintptr_t>(snapshot.depth.data()) % alignof(float)) != 0) {
|
||||
return std::unexpected("depth plane is not aligned for float access");
|
||||
}
|
||||
|
||||
return record::RawDepthMapView{
|
||||
.timestamp_ns = snapshot.metadata.timestamp_ns,
|
||||
.width = depth_info.width,
|
||||
.height = depth_info.height,
|
||||
.source_unit = snapshot.depth_unit,
|
||||
.pixels = std::span<const float>(
|
||||
reinterpret_cast<const float *>(snapshot.depth.data()),
|
||||
pixel_count),
|
||||
};
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
Status publish_access_units(
|
||||
const RuntimeConfig &config,
|
||||
@@ -338,6 +376,7 @@ int run_pipeline(const RuntimeConfig &config) {
|
||||
std::optional<ipc::FrameInfo> active_info{};
|
||||
std::optional<ipc::FrameInfo> restart_target_info{};
|
||||
bool restart_pending{false};
|
||||
bool warned_unknown_depth_unit{false};
|
||||
|
||||
const auto restart_backend = [&](std::string_view reason, std::optional<ipc::FrameInfo> target_info) {
|
||||
if (started) {
|
||||
@@ -352,6 +391,7 @@ int run_pipeline(const RuntimeConfig &config) {
|
||||
started = false;
|
||||
restart_pending = true;
|
||||
restart_target_info = target_info;
|
||||
warned_unknown_depth_unit = false;
|
||||
rtmp_output.reset();
|
||||
};
|
||||
|
||||
@@ -404,6 +444,7 @@ int run_pipeline(const RuntimeConfig &config) {
|
||||
started = true;
|
||||
restart_pending = false;
|
||||
restart_target_info.reset();
|
||||
warned_unknown_depth_unit = false;
|
||||
active_info = target_info;
|
||||
return {};
|
||||
};
|
||||
@@ -510,6 +551,29 @@ int run_pipeline(const RuntimeConfig &config) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (mcap_sink.has_value() && !snapshot->depth.empty()) {
|
||||
if (snapshot->depth_unit == ipc::DepthUnit::Unknown) {
|
||||
if (!warned_unknown_depth_unit) {
|
||||
spdlog::warn("pipeline depth plane present but depth_unit is unknown; skipping depth MCAP recording");
|
||||
warned_unknown_depth_unit = true;
|
||||
}
|
||||
} else {
|
||||
auto depth_map = make_depth_map_view(*snapshot);
|
||||
if (!depth_map) {
|
||||
const auto reason = "pipeline depth snapshot invalid: " + depth_map.error();
|
||||
restart_backend(reason, active_info);
|
||||
continue;
|
||||
}
|
||||
|
||||
auto write_depth = mcap_sink->write_depth_map(*depth_map);
|
||||
if (!write_depth) {
|
||||
const auto reason = "pipeline depth MCAP write failed: " + write_depth.error();
|
||||
restart_backend(reason, active_info);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
stats.pushed_frames += 1;
|
||||
auto drain = drain_encoder(
|
||||
config,
|
||||
|
||||
Reference in New Issue
Block a user