WPILOG Writer implementation

akit/src/main/native/cpp/wpilog/WPILOGWriter.cpp

@@ -9,61 +9,62 @@
 #include <iomanip>
 #include <iostream>
 #include <fstream>
+#include <chrono>
 #include <filesystem>
 #include <frc/DriverStation.h>
 #include <frc/RobotController.h>
+#include <wpi/DataLogWriter.h>
+#include <frc/Timer.h>
 #include "akit/wpilog/WPILOGWriter.h"
 #include "akit/wpilog/WPILOGConstants.h"
 #include "akit/Logger.h"
 
 using namespace akit::wpilog;
 namespace fs = std::filesystem;
 
-WPILOGWriter::WPILOGWriter(std::string path,
+WPILOGWriter::WPILOGWriter(std::string_view path,
 		AdvantageScopeOpenBehavior openBehavior) : openBehavior { openBehavior } {
 	std::mt19937 gen { std::random_device { }() };
 	std::uniform_int_distribution dis { 0, 0xFFFF };
+
 	std::stringstream ss;
 	for (int i = 0; i < 4; i++)
 		ss << std::hex << std::setw(4) << std::setfill('0') << dis(gen);
+
 	randomIdentifier = ss.str();
 
-	fs::path fsPath { path };
-	if (fsPath.extension() == ".wpilog") {
-		folder = fsPath.parent_path().string();
-		filename = fsPath.filename().string();
+	fs::path p { path };
+	if (p.extension() == ".wpilog") {
+		folder = p.parent_path();
+		filename = p.filename();
 		autoRename = false;
 	} else {
-		folder = path;
+		folder = p;
 		filename = "akit_" + randomIdentifier + ".wpilog";
 		autoRename = true;
 	}
 }
 
 void WPILOGWriter::Start() {
-	fs::path logFolder { folder };
-	if (!fs::exists(logFolder))
-		fs::create_directories(logFolder);
-
-	fs::path logFile { logFolder / filename };
-	if (fs::exists(logFile))
-		fs::remove(logFile);
-
-	std::cout << "[AdvantageKit] Logging to \"" << logFile.string() << "\"\n";
-
-	std::error_code code;
+	fs::create_directories (folder);
+	fs::remove (filename);
+	std::cout << "[AdvantageKit] Logging to \"" << folder / filename << "\"\n";
+	std::error_code ec;
 	log = std::make_unique < wpi::log::DataLogWriter
-			> (logFile.string(), code, WPILOGConstants::EXTRA_HEADER);
-
+			> ((folder / filename).string(), ec, WPILOGConstants::EXTRA_HEADER);
+	if (ec) {
+		FRC_ReportError(frc::err::Error,
+				"[AdvantageKit] Failed to open output log file.");
+		return;
+	}
 	isOpen = true;
 	timestampID =
 			log->Start(TIMESTAMP_KEY,
 					LogTable::WPILOG_TYPES[static_cast<int>(LogTable::LoggableType::Integer)]);
-	lastTable = LogTable { 0_s };
 }
 
 void WPILOGWriter::End() {
-	log.release();
+	log.reset();
 
 	bool shouldOpen = false;
 	switch (openBehavior) {
@@ -77,12 +78,11 @@ void WPILOGWriter::End() {
 	default:
 		break;
 	}
+
 	if (shouldOpen) {
-		std::string fullLogPath =
-				fs::absolute(fs::path { folder } / filename).string();
-		fs::path advantageScopeTempPath = fs::temp_directory_path()
-				/ ADVANTAGESCOPE_FILE_NAME;
-		std::ofstream writer { advantageScopeTempPath };
+		fs::path fullLogPath = fs::absolute(folder / filename);
+		fs::path tempPath = fs::temp_directory_path() / ADVANTAGESCOPE_FILENAME;
+		std::ofstream writer { tempPath };
 		writer << fullLogPath;
 		std::cout << "[AdvantageKit] Log sent to AdvantageScope.\n";
 	}
@@ -93,21 +93,19 @@ void WPILOGWriter::PutTable(LogTable &table) {
 		return;
 
 	if (autoRename) {
-		// Update timestamp
 		if (!logDate) {
 			if ((table.Get("DriverStation/DSAttached", false)
 					&& table.Get("SystemStats/SystemTimeValid", false))
+					|| frc::RobotBase::IsSimulation())
+				dsAttachedTime = frc::Timer::GetFPGATimestamp();
+			else if (frc::Timer::GetFPGATimestamp() - *dsAttachedTime
+					> TIMESTAMP_UPDATE_DELAY
 					|| frc::RobotBase::IsSimulation()) {
-				if (!dsAttachedTime)
-					dsAttachedTime = frc::RobotController::GetFPGATime()
-							/ 1000000;
-				else if (frc::RobotController::GetFPGATime() / 1000000
-						- *dsAttachedTime > TIMESTAMP_UPDATE_DELAY
-						|| frc::RobotBase::IsSimulation())
-					logDate = std::chrono::system_clock::now();
-			} else
-				dsAttachedTime.reset();
-		}
+				std::time_t now = std::time(nullptr);
+				logDate = *std::localtime(&now);
+			}
+		} else
+			dsAttachedTime.reset();
 
 		frc::DriverStation::MatchType matchType = frc::DriverStation::kNone;
 		switch (table.Get("DriverStation/MatchType", 0)) {
@@ -121,8 +119,8 @@ void WPILOGWriter::PutTable(LogTable &table) {
 			matchType = frc::DriverStation::kElimination;
 			break;
 		}
+
 		if (logMatchText.empty() && matchType != frc::DriverStation::kNone) {
-			logMatchText = "";
 			switch (matchType) {
 			case frc::DriverStation::kPractice:
 				logMatchText = "p";
@@ -140,11 +138,132 @@ void WPILOGWriter::PutTable(LogTable &table) {
 					table.Get("DriverStation/MatchNumber", 0));
 		}
 
+		std::stringstream newFilenameBuilder;
+		newFilenameBuilder << "akit_";
+		if (!logDate)
+			newFilenameBuilder << randomIdentifier;
+		else
+			newFilenameBuilder
+					<< std::put_time(&(*logDate), TIME_FORMATTER.data());
+
 		std::string eventName = table.Get("DriverStation/EventName",
 				std::string { "" });
 		std::transform(eventName.begin(), eventName.end(), eventName.begin(),
-				::tolower);
+				[](unsigned char c) {
+					return std::tolower(c);
+				});
+		if (!eventName.empty())
+			newFilenameBuilder << "_" << eventName;
+		if (!logMatchText.empty())
+			newFilenameBuilder << "_" << logMatchText;
+		newFilenameBuilder << ".wpilog";
+		fs::path newFilename = newFilenameBuilder.str();
+		if (newFilename != filename) {
+			fs::path logPath = folder / newFilename;
+			std::cout << "[AdvantageKit] Renaming log to \"" << logPath << "\"";
 
-		std::string newFilename = "akit_";
+			fs::rename(folder / filename, folder / newFilename);
+			filename = newFilename;
+		}
 	}
+
+	log->AppendInteger(timestampID,
+			table.GetTimestamp().convert<units::microsecond>().value(),
+			table.GetTimestamp().convert<units::microsecond>().value());
+
+	auto newMap = table.GetAll(false);
+	auto oldMap = lastTable.GetAll(false);
+
+	for (auto field : newMap) {
+		LogTable::LoggableType type = field.second.type;
+		std::string unit = field.second.unitStr;
+		bool appendData = false;
+		if (!entryIDs.contains(field.first)) {
+			std::string metadata {
+					unit.empty() ?
+							WPILOGConstants::ENTRY_METADATA :
+							GetUnitMetadata(unit) };
+			entryIDs[field.first] = log->Start(field.first,
+					LogTable::WPILOG_TYPES[static_cast<int>(field.second.type)],
+					metadata);
+			entryTypes[field.first] = type;
+			if (!unit.empty())
+				entryUnits[field.first] = unit;
+			appendData = true;
+		} else if (field.second != oldMap.at(field.first))
+			appendData = true;
+
+		if (appendData) {
+			int id = entryIDs[field.first];
+
+			if (!unit.empty() && unit != entryUnits[field.first]) {
+				log->SetMetadata(id, GetUnitMetadata(unit),
+						table.GetTimestamp().convert<units::microsecond>().value());
+				entryUnits[field.first] = unit;
+			}
+
+			size_t timestamp =
+					table.GetTimestamp().convert<units::microsecond>().value();
+			switch (field.second.type) {
+			case LogTable::LoggableType::Raw: {
+				auto raw = field.second.GetRaw();
+				std::vector < uint8_t > data;
+				std::transform(raw.begin(), raw.end(), data.begin(),
+						[](std::byte b) {
+							return static_cast<unsigned char>(b);
+						});
+				log->AppendRaw(id, data, timestamp);
+				break;
+			}
+			case LogTable::LoggableType::Boolean:
+				log->AppendBoolean(id, field.second.GetBoolean(), timestamp);
+				break;
+			case LogTable::LoggableType::Integer:
+				log->AppendInteger(id, field.second.GetInteger(), timestamp);
+				break;
+			case LogTable::LoggableType::Float:
+				log->AppendFloat(id, field.second.GetFloat(), timestamp);
+				break;
+			case LogTable::LoggableType::Double:
+				log->AppendDouble(id, field.second.GetDouble(), timestamp);
+				break;
+			case LogTable::LoggableType::String:
+				log->AppendString(id, field.second.GetString(), timestamp);
+				break;
+			case LogTable::LoggableType::BooleanArray: {
+				auto arr = field.second.GetBooleanArray();
+				log->AppendBooleanArray(id, std::vector<uint8_t> { arr.begin(),
+						arr.end() }, timestamp);
+				break;
+			}
+			case LogTable::LoggableType::IntegerArray: {
+				auto arr = field.second.GetIntegerArray();
+				log->AppendIntegerArray(id, std::vector<int64_t> { arr.begin(),
+						arr.end() }, timestamp);
+				break;
+			}
+			case LogTable::LoggableType::FloatArray:
+				log->AppendFloatArray(id, field.second.GetFloatArray(),
+						timestamp);
+				break;
+			case LogTable::LoggableType::DoubleArray:
+				log->AppendDoubleArray(id, field.second.GetDoubleArray(),
+						timestamp);
+				break;
+			case LogTable::LoggableType::StringArray:
+				log->AppendStringArray(id, field.second.GetStringArray(),
+						timestamp);
+				break;
+			}
+		}
+	}
+
+	log->Flush();
+	lastTable = table;
+}
+
+std::string WPILOGWriter::GetUnitMetadata(std::string_view unit) {
+	std::string metadata { WPILOGConstants::ENTRY_METADATA_UNITS };
+	metadata.replace(metadata.find("$UNITSTR"), 8, unit);
+	return metadata;
 }

akit/src/main/native/include/akit/wpilog/WPILOGConstants.h

@@ -15,7 +15,7 @@ namespace wpilog {
 class WPILOGConstants {
 public:
 	static constexpr std::string_view EXTRA_HEADER = "AdvantageKit";
-	static constexpr std::string_view EXTRA_METADATA =
+	static constexpr std::string_view ENTRY_METADATA =
 			"{\"source\":\"AdvantageKit\"}";
 	static constexpr std::string_view ENTRY_METADATA_UNITS =
 			"{\"source\":\"AdvantageKit\",\"unit\":\"$UNITSTR\"}";

akit/src/main/native/include/akit/wpilog/WPILOGReader.h

@@ -13,7 +13,7 @@ namespace akit {
 
 namespace wpilog {
 
-class WPILOGReader: LogReplaySource {
+class WPILOGReader: public LogReplaySource {
 public:
 	WPILOGReader(std::string filename) : filename { filename } {
 	}

akit/src/main/native/include/akit/wpilog/WPILOGWriter.h

@@ -6,6 +6,7 @@
 // at the root directory of this project.
 
 #pragma once
+#include <filesystem>
 #include <wpi/DataLogWriter.h>
 #include <frc/RobotBase.h>
 #include "akit/LogDataReceiver.h"
@@ -17,27 +18,12 @@ namespace wpilog {
 class WPILOGWriter: public LogDataReceiver {
 public:
 	enum class AdvantageScopeOpenBehavior {
-		ALWAYS, AUTO, NEVER
+		ALWAYS, AUTO, NEVER,
 	};
 
-	WPILOGWriter(std::string path, AdvantageScopeOpenBehavior openBehavior);
-
-	WPILOGWriter(std::string path) : WPILOGWriter { path,
-			AdvantageScopeOpenBehavior::AUTO } {
-	}
-
-	WPILOGWriter(AdvantageScopeOpenBehavior openBehavior) : WPILOGWriter {
-			std::string {
-					frc::RobotBase::IsSimulation() ?
-							DEFAULT_PATH_SIM : DEFAULT_PATH_RIO }, openBehavior } {
-	}
-
-	WPILOGWriter() : WPILOGWriter {
-			std::string {
-					frc::RobotBase::IsSimulation() ?
-							DEFAULT_PATH_SIM : DEFAULT_PATH_RIO },
-			AdvantageScopeOpenBehavior::AUTO } {
-	}
+	WPILOGWriter(std::string_view path = DEFAULT_PATH,
+			AdvantageScopeOpenBehavior openBehavior =
+					AdvantageScopeOpenBehavior::AUTO);
 
 	void Start() override;
 
@@ -46,29 +32,30 @@ class WPILOGWriter: public LogDataReceiver {
 	void PutTable(LogTable &table) override;
 
 private:
-	static constexpr double TIMESTAMP_UPDATE_DELAY = 5;
-	static constexpr std::string_view DEFAULT_PATH_RIO = "/U/logs";
-	static constexpr std::string_view DEFAULT_PATH_SIM = "logs";
-	static constexpr std::string_view ADVANTAGESCOPE_FILE_NAME =
+	static constexpr units::second_t TIMESTAMP_UPDATE_DELAY = 5_s;
+	static constexpr std::string_view DEFAULT_PATH =
+			frc::RobotBase::IsSimulation() ? "logs" : "/U/logs";
+	static constexpr std::string_view TIME_FORMATTER = "%y-%m-%d_%H-%M-%S";
+	static constexpr std::string_view ADVANTAGESCOPE_FILENAME =
 			"ascope-log-path.txt";
 
-	std::string folder;
-	std::string filename;
-	std::string randomIdentifier;
-	std::optional<double> dsAttachedTime;
+	static std::string GetUnitMetadata(std::string_view unit);
 
+	bool isOpen = false;
 	bool autoRename;
-	std::optional<std::chrono::system_clock::time_point> logDate;
+	int timestampID;
+	LogTable lastTable { 0_s };
+	AdvantageScopeOpenBehavior openBehavior;
 	std::string logMatchText;
-
+	std::string randomIdentifier;
+	std::filesystem::path folder;
+	std::filesystem::path filename;
 	std::unique_ptr<wpi::log::DataLogWriter> log;
-	bool isOpen = false;
-	AdvantageScopeOpenBehavior openBehavior;
-	std::optional<LogTable> lastTable;
-	int timestampID;
 	std::unordered_map<std::string, int> entryIDs;
 	std::unordered_map<std::string, LogTable::LoggableType> entryTypes;
 	std::unordered_map<std::string, std::string> entryUnits;
+	std::optional<std::tm> logDate;
+	std::optional<units::millisecond_t> dsAttachedTime;
 };
 
 }