salmanoff/stimBuffApis/livoxGen1/livoxPcloudFrameDumper.cpp

#include "livoxPcloudFrameDumper.h"

#include <chrono>
#include <cmath>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <limits>
#include <sstream>
#include <stdexcept>

namespace smo {
namespace stim_buff {

namespace {

constexpr const char* kDumpDirParamName = "pcloud-dump-dir";
constexpr const char* kPcdExtension = ".pcd";
constexpr std::size_t kXyzComponentsPerPoint = 3;

std::string makePcdHeader(std::size_t width, std::size_t height)
{
	std::ostringstream oss;
	oss << "# .PCD v0.7 - Point Cloud Data file format\n";
	oss << "VERSION 0.7\n";
	oss << "FIELDS x y z\n";
	oss << "SIZE 4 4 4\n";
	oss << "TYPE F F F\n";
	oss << "COUNT 1 1 1\n";
	oss << "WIDTH " << width << "\n";
	oss << "HEIGHT " << height << "\n";
	oss << "VIEWPOINT 0 0 0 1 0 0 0\n";
	oss << "POINTS " << (width * height) << "\n";
	oss << "DATA binary\n";
	return oss.str();
}

float sanitizeCoordinate(float coordinate)
{
	if (coordinate == 0.0f)
	{
		return std::numeric_limits<float>::quiet_NaN();
	}

	return coordinate;
}

bool pointIsZero(float x, float y, float z)
{
	return x == 0.0f && y == 0.0f && z == 0.0f;
}

} // namespace

LivoxPcloudFrameDumper::LivoxPcloudFrameDumper(
	const std::shared_ptr<device::DeviceAttachmentSpec>& deviceAttachmentSpec)
: enabled(false),
deviceSelector(deviceAttachmentSpec ? deviceAttachmentSpec->deviceSelector : ""),
dumpDirectory()
{
	if (!deviceAttachmentSpec)
	{
		throw std::runtime_error(
			std::string(__func__) + ": deviceAttachmentSpec is null");
	}

	std::string dumpDirParam = parseDumpDirParam(
		deviceAttachmentSpec->stimBuffApiParams);
	if (dumpDirParam.empty())
	{
		return;
	}

	enabled = true;
	dumpDirectory = resolveDumpDirectory(dumpDirParam);
}

void LivoxPcloudFrameDumper::prepareForRun() const
{
	if (!enabled)
	{
		return;
	}

	std::filesystem::create_directories(dumpDirectory);
}

void LivoxPcloudFrameDumper::dumpProducedFrame(
	const livoxProto1::Device& device,
	const StagingBuffer& collationBuffer,
	const sscl::AsynchronousLoop& frameAssemblyResult) const
{
	if (!enabled)
	{
		return;
	}

	std::size_t nSucceeded = frameAssemblyResult.nSucceeded.load();
	if (nSucceeded == 0)
	{
		return;
	}

	std::size_t pointsPerDgram = livoxProto1::Device::getNPointsPerDgram(
		static_cast<int>(device.currentReturnMode));
	if (pointsPerDgram == 0)
	{
		throw std::runtime_error(
			std::string(__func__) + ": pointsPerDgram resolved to 0");
	}

	std::filesystem::path outputPath = makeUniqueFramePath(
		dumpDirectory, deviceSelector);
	writeBinaryPcdFile(
		outputPath, collationBuffer, nSucceeded, pointsPerDgram);
}

std::string LivoxPcloudFrameDumper::parseDumpDirParam(
	const std::vector<std::pair<std::string, std::string>>& params)
{
	for (auto it = params.rbegin(); it != params.rend(); ++it)
	{
		if (it->first == kDumpDirParamName)
		{
			return it->second;
		}
	}

	return "";
}

std::filesystem::path LivoxPcloudFrameDumper::resolveDumpDirectory(
	const std::string& dumpDirParam)
{
	std::filesystem::path dumpDirPath(dumpDirParam);
	if (dumpDirPath.is_absolute())
	{
		return dumpDirPath;
	}

	return std::filesystem::current_path() / dumpDirPath;
}

std::string LivoxPcloudFrameDumper::makeTimestampStem(
	const std::string& deviceSelector)
{
	auto now = std::chrono::system_clock::now();
	std::time_t nowTime = std::chrono::system_clock::to_time_t(now);
	std::tm nowTm = *std::localtime(&nowTime);

	std::ostringstream oss;
	oss << deviceSelector << "-"
		<< std::put_time(&nowTm, "%Y-%m-%d--%H:%M:%S");
	return oss.str();
}

std::filesystem::path LivoxPcloudFrameDumper::makeUniqueFramePath(
	const std::filesystem::path& dumpDir,
	const std::string& deviceSelector)
{
	std::string stem = makeTimestampStem(deviceSelector);
	std::filesystem::path candidate = dumpDir / (stem + kPcdExtension);
	if (!std::filesystem::exists(candidate))
	{
		return candidate;
	}

	for (std::size_t suffix = 1; ; ++suffix)
	{
		std::ostringstream oss;
		oss << stem << "--" << std::setw(3) << std::setfill('0') << suffix
			<< kPcdExtension;
		candidate = dumpDir / oss.str();
		if (!std::filesystem::exists(candidate))
		{
			return candidate;
		}
	}
}

void LivoxPcloudFrameDumper::writeBinaryPcdFile(
	const std::filesystem::path& outputPath,
	const StagingBuffer& collationBuffer,
	std::size_t nSucceeded,
	std::size_t pointsPerDgram)
{
	std::ofstream output(outputPath, std::ios::binary);
	if (!output)
	{
		throw std::runtime_error(
			std::string(__func__) + ": failed to open " + outputPath.string());
	}

	output << makePcdHeader(pointsPerDgram, nSucceeded);

	struct iovec collationIov = collationBuffer.getClEngineIovec();
	const auto* bufferBase = static_cast<const std::uint8_t*>(collationIov.iov_base);
	const std::size_t rowNBytes =
		pointsPerDgram * kXyzComponentsPerPoint * sizeof(float);
	const std::size_t slotStrideNBytes = collationBuffer.slotStrideNBytes;

	for (std::size_t rowIndex = 0; rowIndex < nSucceeded; ++rowIndex)
	{
		const auto* rowBase = reinterpret_cast<const float*>(
			bufferBase + (rowIndex * slotStrideNBytes));

		for (std::size_t pointIndex = 0; pointIndex < pointsPerDgram; ++pointIndex)
		{
			const std::size_t pointOffset = pointIndex * kXyzComponentsPerPoint;
			float x = rowBase[pointOffset + 0];
			float y = rowBase[pointOffset + 1];
			float z = rowBase[pointOffset + 2];

			if (pointIsZero(x, y, z))
			{
				x = sanitizeCoordinate(x);
				y = sanitizeCoordinate(y);
				z = sanitizeCoordinate(z);
			}

			output.write(reinterpret_cast<const char*>(&x), sizeof(float));
			output.write(reinterpret_cast<const char*>(&y), sizeof(float));
			output.write(reinterpret_cast<const char*>(&z), sizeof(float));
		}

		(void)rowNBytes;
	}

	if (!output)
	{
		throw std::runtime_error(
			std::string(__func__) + ": failed while writing "
			+ outputPath.string());
	}
}

} // namespace stim_buff
} // namespace smo