feat(record): add raw ZED body MCAP capture

src/pipeline/pipeline_runtime.cpp

@@ -8,6 +8,7 @@
 #include "cvmmap_streamer/record/mcap_record_sink.hpp"
 
 #include <cvmmap/client.hpp>
+#include <cvmmap/parser.hpp>
 
 #include <chrono>
 #include <cstddef>
@@ -59,21 +60,23 @@ constexpr int exit_code(PipelineExitCode code) {
 struct ResolvedInputEndpoints {
 	std::string shm_name;
 	std::string zmq_endpoint;
+	std::string body_zmq_endpoint;
 };
 
 [[nodiscard]]
 std::expected<ResolvedInputEndpoints, std::string> resolve_input_endpoints(const RuntimeConfig &config) {
 	try {
 		auto target = cvmmap::resolve_cvmmap_target_or_throw(config.input.uri);
-		spdlog::info(
-			"pipeline input resolved: uri='{}' shm_name='{}' zmq_endpoint='{}'",
-			config.input.uri,
-			target.shm_name,
-			target.zmq_addr);
-		return ResolvedInputEndpoints{
-			.shm_name = target.shm_name,
-			.zmq_endpoint = target.zmq_addr,
-		};
+			spdlog::info(
+				"pipeline input resolved: uri='{}' shm_name='{}' zmq_endpoint='{}'",
+				config.input.uri,
+				target.shm_name,
+				target.zmq_addr);
+			return ResolvedInputEndpoints{
+				.shm_name = target.shm_name,
+				.zmq_endpoint = target.zmq_addr,
+				.body_zmq_endpoint = target.zmq_body_addr,
+			};
 	} catch (const std::exception &e) {
 		return std::unexpected(std::string("invalid cvmmap input URI: ") + e.what());
 	}
@@ -235,6 +238,14 @@ std::expected<record::RawDepthMapView, std::string> make_depth_map_view(const ip
 	};
 }
 
+[[nodiscard]]
+std::uint64_t body_tracking_timestamp_ns(const cvmmap::body_tracking_frame_t &frame) {
+	if (frame.header.timestamp_ns != 0) {
+		return frame.header.timestamp_ns;
+	}
+	return frame.header.sdk_timestamp_ns;
+}
+
 [[nodiscard]]
 Status publish_access_units(
 	const RuntimeConfig &config,
@@ -354,13 +365,24 @@ int run_pipeline(const RuntimeConfig &config) {
 	zmq::socket_t subscriber(zmq_ctx, zmq::socket_type::sub);
 	try {
 		subscriber.set(zmq::sockopt::subscribe, "");
-		subscriber.set(zmq::sockopt::rcvtimeo, 20);
 		subscriber.connect(input_endpoints->zmq_endpoint);
 	} catch (const zmq::error_t &e) {
 		spdlog::error("pipeline subscribe failed on '{}': {}", input_endpoints->zmq_endpoint, e.what());
 		return exit_code(PipelineExitCode::SubscriberError);
 	}
 
+	std::optional<zmq::socket_t> body_subscriber{};
+	if (config.record.mcap.enabled) {
+		try {
+			body_subscriber.emplace(zmq_ctx, zmq::socket_type::sub);
+			body_subscriber->set(zmq::sockopt::subscribe, "");
+			body_subscriber->connect(input_endpoints->body_zmq_endpoint);
+		} catch (const zmq::error_t &e) {
+			spdlog::error("pipeline body subscribe failed on '{}': {}", input_endpoints->body_zmq_endpoint, e.what());
+			return exit_code(PipelineExitCode::SubscriberError);
+		}
+	}
+
 	std::optional<protocol::UdpRtpPublisher> rtp_publisher{};
 	std::optional<protocol::RtmpOutput> rtmp_output{};
 	std::optional<record::McapRecordSink> mcap_sink{};
@@ -464,9 +486,22 @@ int run_pipeline(const RuntimeConfig &config) {
 			restart_backend(reason, active_info);
 		}
 
-		zmq::message_t message;
-		const auto recv_result = subscriber.recv(message, zmq::recv_flags::none);
-		if (!recv_result) {
+		std::array<zmq::pollitem_t, 2> poll_items{{
+			{subscriber.handle(), 0, ZMQ_POLLIN, 0},
+			{body_subscriber ? body_subscriber->handle() : nullptr, 0, ZMQ_POLLIN, 0},
+		}};
+		try {
+			zmq::poll(poll_items, std::chrono::milliseconds{20});
+		} catch (const zmq::error_t &e) {
+			spdlog::error("pipeline poll failed: {}", e.what());
+			return exit_code(PipelineExitCode::SubscriberError);
+		}
+
+		const bool frame_socket_ready = (poll_items[0].revents & ZMQ_POLLIN) != 0;
+		const bool body_socket_ready =
+			body_subscriber.has_value() &&
+			(poll_items[1].revents & ZMQ_POLLIN) != 0;
+		if (!frame_socket_ready && !body_socket_ready) {
 			const auto now = std::chrono::steady_clock::now();
 			if (restart_pending && restart_target_info) {
 				auto start_result = attempt_backend_start(*restart_target_info);
@@ -497,6 +532,53 @@ int run_pipeline(const RuntimeConfig &config) {
 			continue;
 		}
 
+		if (body_socket_ready && body_subscriber) {
+			while (true) {
+				zmq::message_t body_message;
+				const auto recv_body = body_subscriber->recv(body_message, zmq::recv_flags::dontwait);
+				if (!recv_body) {
+					break;
+				}
+
+				last_event = std::chrono::steady_clock::now();
+				auto body_bytes = std::span<const std::uint8_t>(
+					static_cast<const std::uint8_t *>(body_message.data()),
+					body_message.size());
+				if (body_bytes.empty()) {
+					continue;
+				}
+					if (!mcap_sink) {
+						continue;
+					}
+
+				auto parsed_body = cvmmap::parse_body_tracking_message(body_bytes);
+				if (!parsed_body) {
+					spdlog::warn("pipeline body packet parse error: {}", parsed_body.error());
+					continue;
+				}
+
+				auto write_body = mcap_sink->write_body_tracking_message(record::RawBodyTrackingMessageView{
+					.timestamp_ns = body_tracking_timestamp_ns(*parsed_body),
+					.bytes = body_bytes,
+				});
+				if (!write_body) {
+					const auto reason = "pipeline body MCAP write failed: " + write_body.error();
+					restart_backend(reason, active_info);
+					break;
+				}
+			}
+		}
+
+		if (!frame_socket_ready) {
+			continue;
+		}
+
+		zmq::message_t message;
+		const auto recv_result = subscriber.recv(message, zmq::recv_flags::dontwait);
+		if (!recv_result) {
+			continue;
+		}
+
 		last_event = std::chrono::steady_clock::now();
 		auto bytes = std::span<const std::uint8_t>(
 			static_cast<const std::uint8_t *>(message.data()),