crosstyan/cvmmap-streamer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(mcap): add paced replay tooling

Browse Source
This commit is contained in:
crosstyan
2026-03-11 15:51:38 +08:00
parent bc1b619dee
commit ed3f32ff6e
7 changed files with 605 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/record/mcap_record_sink.cpp
+161 -3
View File
@@ -12,9 +12,11 @@
#include <cstdint>
#include <expected>
#include <memory>
#include <span>
#include <string>
#include <string_view>
#include <utility>
#include <vector>
namespace cvmmap_streamer::record {
@@ -46,6 +48,134 @@ google::protobuf::Timestamp to_proto_timestamp(std::uint64_t timestamp_ns) {
return timestamp;
}
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(0x01);
}
[[nodiscard]]
std::expected<std::uint16_t, std::string> read_be16(std::span<const std::uint8_t> bytes, std::size_t offset) {
if (offset + 2 > bytes.size()) {
return std::unexpected("decoder config truncated");
}
return static_cast<std::uint16_t>((static_cast<std::uint16_t>(bytes[offset]) << 8) | bytes[offset + 1]);
}
[[nodiscard]]
bool looks_like_annexb(std::span<const std::uint8_t> bytes) {
if (bytes.size() >= 4 &&
bytes[0] == 0x00 &&
bytes[1] == 0x00 &&
bytes[2] == 0x00 &&
bytes[3] == 0x01) {
return true;
}
return bytes.size() >= 3 &&
bytes[0] == 0x00 &&
bytes[1] == 0x00 &&
bytes[2] == 0x01;
}
[[nodiscard]]
std::expected<std::vector<std::uint8_t>, std::string> avcc_to_annexb(std::span<const std::uint8_t> decoder_config) {
if (decoder_config.size() < 7 || decoder_config[0] != 1) {
return std::unexpected("invalid AVC decoder config");
}
std::vector<std::uint8_t> annexb{};
std::size_t offset = 5;
const auto sps_count = static_cast<std::size_t>(decoder_config[offset++] & 0x1fu);
for (std::size_t i = 0; i < sps_count; ++i) {
auto size = read_be16(decoder_config, offset);
if (!size) {
return std::unexpected(size.error());
}
offset += 2;
if (offset + *size > decoder_config.size()) {
return std::unexpected("invalid AVC decoder config payload");
}
append_start_code(annexb);
annexb.insert(annexb.end(), decoder_config.begin() + static_cast<std::ptrdiff_t>(offset), decoder_config.begin() + static_cast<std::ptrdiff_t>(offset + *size));
offset += *size;
}
if (offset >= decoder_config.size()) {
return std::unexpected("invalid AVC decoder config: missing PPS count");
}
const auto pps_count = static_cast<std::size_t>(decoder_config[offset++]);
for (std::size_t i = 0; i < pps_count; ++i) {
auto size = read_be16(decoder_config, offset);
if (!size) {
return std::unexpected(size.error());
}
offset += 2;
if (offset + *size > decoder_config.size()) {
return std::unexpected("invalid AVC decoder config payload");
}
append_start_code(annexb);
annexb.insert(annexb.end(), decoder_config.begin() + static_cast<std::ptrdiff_t>(offset), decoder_config.begin() + static_cast<std::ptrdiff_t>(offset + *size));
offset += *size;
}
return annexb;
}
[[nodiscard]]
std::expected<std::vector<std::uint8_t>, std::string> hvcc_to_annexb(std::span<const std::uint8_t> decoder_config) {
if (decoder_config.size() < 23 || decoder_config[0] != 1) {
return std::unexpected("invalid HEVC decoder config");
}
std::vector<std::uint8_t> annexb{};
std::size_t offset = 22;
const auto array_count = static_cast<std::size_t>(decoder_config[offset++]);
for (std::size_t array_index = 0; array_index < array_count; ++array_index) {
if (offset + 3 > decoder_config.size()) {
return std::unexpected("invalid HEVC decoder config arrays");
}
offset += 1;
auto nal_count = read_be16(decoder_config, offset);
if (!nal_count) {
return std::unexpected(nal_count.error());
}
offset += 2;
for (std::size_t nal_index = 0; nal_index < *nal_count; ++nal_index) {
auto nal_size = read_be16(decoder_config, offset);
if (!nal_size) {
return std::unexpected(nal_size.error());
}
offset += 2;
if (offset + *nal_size > decoder_config.size()) {
return std::unexpected("invalid HEVC decoder config payload");
}
append_start_code(annexb);
annexb.insert(annexb.end(), decoder_config.begin() + static_cast<std::ptrdiff_t>(offset), decoder_config.begin() + static_cast<std::ptrdiff_t>(offset + *nal_size));
offset += *nal_size;
}
}
return annexb;
}
[[nodiscard]]
std::expected<std::vector<std::uint8_t>, std::string> decoder_config_to_annexb(
CodecType codec,
std::span<const std::uint8_t> decoder_config) {
if (decoder_config.empty()) {
return std::vector<std::uint8_t>{};
}
if (looks_like_annexb(decoder_config)) {
return std::vector<std::uint8_t>(decoder_config.begin(), decoder_config.end());
}
if (codec == CodecType::H265) {
return hvcc_to_annexb(decoder_config);
}
return avcc_to_annexb(decoder_config);
}
}
struct McapRecordSink::State {
@@ -54,6 +184,8 @@ struct McapRecordSink::State {
std::string frame_id{};
mcap::ChannelId channel_id{0};
std::uint32_t sequence{0};
CodecType codec{CodecType::H264};
std::vector<std::uint8_t> keyframe_preamble{};
};
McapRecordSink::~McapRecordSink() {
@@ -74,7 +206,9 @@ McapRecordSink &McapRecordSink::operator=(McapRecordSink &&other) noexcept {
return *this;
}
std::expected<McapRecordSink, std::string> McapRecordSink::create(const RuntimeConfig &config) {
std::expected<McapRecordSink, std::string> McapRecordSink::create(
const RuntimeConfig &config,
const encode::EncodedStreamInfo &stream_info) {
McapRecordSink sink{};
auto state = std::make_unique<State>();
state->path = config.record.mcap.path;
@@ -101,9 +235,27 @@ std::expected<McapRecordSink, std::string> McapRecordSink::create(const RuntimeC
state->channel_id = channel.id;
sink.state_ = state.release();
auto update = sink.update_stream_info(stream_info);
if (!update) {
sink.close();
return std::unexpected(update.error());
}
return sink;
}
std::expected<void, std::string> McapRecordSink::update_stream_info(const encode::EncodedStreamInfo &stream_info) {
if (state_ == nullptr) {
return std::unexpected("MCAP sink is not open");
}
auto decoder_config_annexb = decoder_config_to_annexb(stream_info.codec, stream_info.decoder_config);
if (!decoder_config_annexb) {
return std::unexpected("failed to prepare MCAP keyframe decoder config: " + decoder_config_annexb.error());
}
state_->codec = stream_info.codec;
state_->keyframe_preamble = std::move(*decoder_config_annexb);
return {};
}
std::expected<void, std::string> McapRecordSink::write_access_unit(const encode::EncodedAccessUnit &access_unit) {
if (state_ == nullptr) {
return std::unexpected("MCAP sink is not open");
@@ -113,9 +265,15 @@ std::expected<void, std::string> McapRecordSink::write_access_unit(const encode:
*message.mutable_timestamp() = to_proto_timestamp(access_unit.source_timestamp_ns);
message.set_frame_id(state_->frame_id);
message.set_format(codec_format(access_unit.codec));
std::vector<std::uint8_t> payload{};
if (access_unit.keyframe && !state_->keyframe_preamble.empty()) {
payload.reserve(state_->keyframe_preamble.size() + access_unit.annexb_bytes.size());
payload.insert(payload.end(), state_->keyframe_preamble.begin(), state_->keyframe_preamble.end());
}
payload.insert(payload.end(), access_unit.annexb_bytes.begin(), access_unit.annexb_bytes.end());
message.set_data(
reinterpret_cast<const char *>(access_unit.annexb_bytes.data()),
static_cast<int>(access_unit.annexb_bytes.size()));
reinterpret_cast<const char *>(payload.data()),
static_cast<int>(payload.size()));
std::string serialized{};
if (!message.SerializeToString(&serialized)) {