feat(record): add raw ZED body MCAP capture

src/record/mcap_record_sink.cpp

@@ -27,6 +27,7 @@ namespace {
 
 constexpr float kRvlDepthQuantization = 200.0f;
 constexpr float kMinDepthMaxMeters = 20.0f;
+constexpr std::string_view kBodyTrackingMessageEncoding = "cvmmap.body_tracking.v1";
 
 [[nodiscard]]
 std::string codec_format(CodecType codec) {
@@ -60,6 +61,15 @@ mcap::Compression to_mcap_compression(McapCompression compression) {
 	}
 }
 
+[[nodiscard]]
+mcap::KeyValueMap body_channel_metadata() {
+	return mcap::KeyValueMap{
+		{"packet_layout", "cvmmap_body_tracking_v1"},
+		{"payload_format", "raw"},
+		{"source_transport", "cv-mmap-body-pub"},
+	};
+}
+
 [[nodiscard]]
 google::protobuf::Timestamp to_proto_timestamp(std::uint64_t timestamp_ns) {
 	google::protobuf::Timestamp timestamp{};
@@ -317,10 +327,13 @@ struct McapRecordSink::State {
 	mcap::McapWriter writer{};
 	std::string path{};
 	std::string frame_id{};
+	std::string body_topic{};
 	mcap::ChannelId video_channel_id{0};
 	mcap::ChannelId depth_channel_id{0};
+	mcap::ChannelId body_channel_id{0};
 	std::uint32_t video_sequence{0};
 	std::uint32_t depth_sequence{0};
+	std::uint32_t body_sequence{0};
 	CodecType codec{CodecType::H264};
 	std::vector<std::uint8_t> keyframe_preamble{};
 };
@@ -350,6 +363,7 @@ std::expected<McapRecordSink, std::string> McapRecordSink::create(
 	auto state = std::make_unique<State>();
 	state->path = config.record.mcap.path;
 	state->frame_id = config.record.mcap.frame_id;
+	state->body_topic = config.record.mcap.body_topic;
 
 	mcap::McapWriterOptions options("");
 	options.compression = to_mcap_compression(config.record.mcap.compression);
@@ -488,6 +502,39 @@ std::expected<void, std::string> McapRecordSink::write_depth_map(const RawDepthM
 	return {};
 }
 
+std::expected<void, std::string> McapRecordSink::write_body_tracking_message(const RawBodyTrackingMessageView &body_message) {
+	if (state_ == nullptr) {
+		return std::unexpected("MCAP sink is not open");
+	}
+	if (body_message.bytes.empty()) {
+		return std::unexpected("body tracking payload is empty");
+	}
+
+	if (state_->body_channel_id == 0) {
+		mcap::Channel body_channel(
+			state_->body_topic,
+			kBodyTrackingMessageEncoding,
+			0,
+			body_channel_metadata());
+		state_->writer.addChannel(body_channel);
+		state_->body_channel_id = body_channel.id;
+	}
+
+	mcap::Message record{};
+	record.channelId   = state_->body_channel_id;
+	record.sequence    = state_->body_sequence++;
+	record.logTime     = body_message.timestamp_ns;
+	record.publishTime = body_message.timestamp_ns;
+	record.data        = reinterpret_cast<const std::byte *>(body_message.bytes.data());
+	record.dataSize    = body_message.bytes.size();
+
+	const auto write_status = state_->writer.write(record);
+	if (!write_status.ok()) {
+		return std::unexpected("failed to write MCAP body message: " + write_status.message);
+	}
+	return {};
+}
+
 bool McapRecordSink::is_open() const {
 	return state_ != nullptr;
 }