Skip unreadable tail frames in grid conversion

src/tools/zed_svo_grid_to_mp4.cpp

@@ -315,6 +315,40 @@ std::expected<void, std::string> promote_next_frame(CameraStream &stream) {
 	return fill_next_frame(stream);
 }
 
+[[nodiscard]]
+std::expected<std::uint64_t, std::string> read_last_readable_timestamp(CameraStream &stream) {
+	const auto last_candidate = static_cast<int>(stream.total_frames - 1);
+	std::string last_error{};
+
+	for (int position = last_candidate; position >= 0; --position) {
+		stream.camera->setSVOPosition(position);
+		std::uint64_t timestamp_ns = 0;
+		auto frame = read_into_mat(
+			*stream.camera,
+			stream.runtime,
+			stream.current_frame,
+			std::nullopt,
+			stream.nominal_frame_period_ns,
+			timestamp_ns,
+			stream.source.label);
+		if (frame) {
+			const auto skipped_tail_frames = static_cast<std::uint64_t>(last_candidate - position);
+			if (skipped_tail_frames > 0) {
+				spdlog::warn(
+					"skipping {} unreadable tail frame(s) for {} last_error={}",
+					skipped_tail_frames,
+					stream.source.path.string(),
+					last_error);
+			}
+			return timestamp_ns;
+		}
+		last_error = frame.error();
+	}
+
+	return std::unexpected(
+		"failed to read any trailing frame for " + stream.source.path.string() + ": " + last_error);
+}
+
 [[nodiscard]]
 std::expected<CameraStream, std::string> open_camera_stream(const SourceSpec &source) {
 	CameraStream stream{};
@@ -363,20 +397,11 @@ std::expected<CameraStream, std::string> open_camera_stream(const SourceSpec &so
 	}
 	stream.first_timestamp_ns = first_timestamp_ns;
 
-	stream.camera->setSVOPosition(static_cast<int>(stream.total_frames - 1));
-	std::uint64_t last_timestamp_ns = 0;
-	auto last_frame = read_into_mat(
-		*stream.camera,
-		stream.runtime,
-		stream.current_frame,
-		std::nullopt,
-		stream.nominal_frame_period_ns,
-		last_timestamp_ns,
-		source.label);
-	if (!last_frame) {
-		return std::unexpected(last_frame.error());
+	auto last_timestamp_ns = read_last_readable_timestamp(stream);
+	if (!last_timestamp_ns) {
+		return std::unexpected(last_timestamp_ns.error());
 	}
-	stream.last_timestamp_ns = last_timestamp_ns;
+	stream.last_timestamp_ns = *last_timestamp_ns;
 
 	return stream;
 }