Merge "Change localizer code to send vision pi statuses"
diff --git a/aos/configuration.fbs b/aos/configuration.fbs
index 2b25a92..5283d9e 100644
--- a/aos/configuration.fbs
+++ b/aos/configuration.fbs
@@ -32,6 +32,15 @@
timestamp_logger_nodes:[string] (id: 2);
// Priority to forward data with.
+ // The priority value in SCTP_SS_PRIO is used to determine the order in which
+ // data from different streams is transmitted. Lower priority values indicate
+ // higher priority in sending the data. When the SCTP stack has to choose
+ // which stream's data to send next, it will select the data from the stream
+ // with the highest priority (i.e., the lowest priority value). If two or
+ // more streams have the same priority, SCTP_SS_PRIO falls back to a
+ // round-robin scheduling between these streams. Note that this does not
+ // reserve any potion of the bandwidth. It is only used to determine which
+ // message to send when the system is ready to send a message.
priority:ushort = 100 (id: 3);
// Time to live in nanoseconds before the message is dropped.
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index 1edc142..6cb3740 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -73,6 +73,35 @@
)
cc_library(
+ name = "log_backend",
+ srcs = ["log_backend.cc"],
+ hdrs = ["log_backend.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/time",
+ "//aos/util:file",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/strings",
+ "@com_google_absl//absl/types:span",
+ ],
+)
+
+cc_test(
+ name = "log_backend_test",
+ srcs = ["log_backend_test.cc"],
+ shard_count = 4,
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":log_backend",
+ "//aos/containers:resizeable_buffer",
+ "//aos/testing:googletest",
+ "//aos/testing:tmpdir",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_library(
name = "log_reader_utils",
srcs = [
"log_reader_utils.cc",
@@ -116,6 +145,7 @@
":snappy_encoder",
":buffer_encoder",
":logger_fbs",
+ ":log_backend",
"//aos:uuid",
"//aos:configuration",
"//aos:flatbuffer_merge",
diff --git a/aos/events/logging/log_backend.cc b/aos/events/logging/log_backend.cc
new file mode 100644
index 0000000..a8b7a43
--- /dev/null
+++ b/aos/events/logging/log_backend.cc
@@ -0,0 +1,411 @@
+#include "aos/events/logging/log_backend.h"
+
+#include <dirent.h>
+
+#include <filesystem>
+
+#include "absl/strings/str_cat.h"
+#include "aos/util/file.h"
+#include "glog/logging.h"
+
+DEFINE_bool(direct, false,
+ "If true, write using O_DIRECT and write 512 byte aligned blocks "
+ "whenever possible.");
+DEFINE_bool(
+ sync, false,
+ "If true, sync data to disk as we go so we don't get too far ahead. Also "
+ "fadvise that we are done with the memory once it hits disk.");
+
+namespace aos::logger {
+namespace {
+constexpr const char *kTempExtension = ".tmp";
+}
+
+FileHandler::FileHandler(std::string filename)
+ : filename_(std::move(filename)), supports_odirect_(FLAGS_direct) {}
+
+FileHandler::~FileHandler() { Close(); }
+
+WriteCode FileHandler::OpenForWrite() {
+ iovec_.reserve(10);
+ if (!aos::util::MkdirPIfSpace(filename_, 0777)) {
+ return WriteCode::kOutOfSpace;
+ } else {
+ fd_ = open(filename_.c_str(), O_RDWR | O_CLOEXEC | O_CREAT | O_EXCL, 0774);
+ if (fd_ == -1 && errno == ENOSPC) {
+ return WriteCode::kOutOfSpace;
+ } else {
+ PCHECK(fd_ != -1) << ": Failed to open " << filename_ << " for writing";
+ VLOG(1) << "Opened " << filename_ << " for writing";
+ }
+
+ flags_ = fcntl(fd_, F_GETFL, 0);
+ PCHECK(flags_ >= 0) << ": Failed to get flags for " << this->filename();
+
+ EnableDirect();
+
+ CHECK(std::filesystem::exists(filename_));
+
+ return WriteCode::kOk;
+ }
+}
+
+void FileHandler::EnableDirect() {
+ if (supports_odirect_ && !ODirectEnabled()) {
+ const int new_flags = flags_ | O_DIRECT;
+ // Track if we failed to set O_DIRECT. Note: Austin hasn't seen this call
+ // fail. The write call tends to fail instead.
+ if (fcntl(fd_, F_SETFL, new_flags) == -1) {
+ PLOG(WARNING) << "Failed to set O_DIRECT on " << filename();
+ supports_odirect_ = false;
+ } else {
+ VLOG(1) << "Enabled O_DIRECT on " << filename();
+ flags_ = new_flags;
+ }
+ }
+}
+
+void FileHandler::DisableDirect() {
+ if (supports_odirect_ && ODirectEnabled()) {
+ flags_ = flags_ & (~O_DIRECT);
+ PCHECK(fcntl(fd_, F_SETFL, flags_) != -1) << ": Failed to disable O_DIRECT";
+ VLOG(1) << "Disabled O_DIRECT on " << filename();
+ }
+}
+
+WriteResult FileHandler::Write(
+ const absl::Span<const absl::Span<const uint8_t>> &queue) {
+ iovec_.clear();
+ CHECK_LE(queue.size(), static_cast<size_t>(IOV_MAX));
+ iovec_.resize(queue.size());
+ // Size of the data currently in iovec_.
+ size_t counted_size = 0;
+
+ // Ok, we now need to figure out if we were aligned, and if we were, how much
+ // of the data we are being asked to write is aligned.
+ //
+ // When writing with O_DIRECT, the kernel only will accept writes where the
+ // offset into the file is a multiple of kSector, the data is aligned to
+ // kSector in memory, and the length being written is a multiple of kSector.
+ // Some of the callers use an aligned ResizeableBuffer to generate 512 byte
+ // aligned buffers for this code to find and use.
+ bool aligned = (total_write_bytes_ % kSector) == 0;
+
+ // Index we are filling in next. Keeps resetting back to 0 as we write
+ // intermediates.
+ size_t write_index = 0;
+ for (size_t i = 0; i < queue.size(); ++i) {
+ iovec_[write_index].iov_base = const_cast<uint8_t *>(queue[i].data());
+ iovec_[write_index].iov_len = queue[i].size();
+
+ // Make sure the address is aligned, or give up. This should be uncommon,
+ // but is always possible.
+ if ((reinterpret_cast<size_t>(iovec_[write_index].iov_base) % kSector) !=
+ 0) {
+ // Flush if we were aligned and have data.
+ if (aligned && write_index != 0) {
+ VLOG(1) << "Was aligned, now is not, writing previous data";
+ const auto code =
+ WriteV(iovec_.data(), write_index, true, counted_size);
+ if (code == WriteCode::kOutOfSpace) {
+ return {
+ .code = code,
+ .messages_written = i,
+ };
+ }
+
+ // Now, everything before here has been written. Make an iovec out of
+ // the rest and keep going.
+ write_index = 0;
+ counted_size = 0;
+
+ iovec_[write_index].iov_base = const_cast<uint8_t *>(queue[i].data());
+ iovec_[write_index].iov_len = queue[i].size();
+ }
+ aligned = false;
+ } else {
+ // We are now starting aligned again, and have data worth writing! Flush
+ // what was there before.
+ if (!aligned && iovec_[write_index].iov_len >= kSector &&
+ write_index != 0) {
+ VLOG(1) << "Was not aligned, now is, writing previous data";
+
+ const auto code =
+ WriteV(iovec_.data(), write_index, false, counted_size);
+ if (code == WriteCode::kOutOfSpace) {
+ return {
+ .code = code,
+ .messages_written = i,
+ };
+ }
+
+ // Now, everything before here has been written. Make an iovec out of
+ // the rest and keep going.
+ write_index = 0;
+ counted_size = 0;
+
+ iovec_[write_index].iov_base = const_cast<uint8_t *>(queue[i].data());
+ iovec_[write_index].iov_len = queue[i].size();
+ aligned = true;
+ }
+ }
+
+ // Now, see if the length is a multiple of kSector. The goal is to figure
+ // out if/how much memory we can write out with O_DIRECT so that only the
+ // last little bit is done with non-direct IO to keep it fast.
+ if ((iovec_[write_index].iov_len % kSector) != 0) {
+ VLOG(1) << "Unaligned length " << iovec_[write_index].iov_len << " on "
+ << filename();
+ // If we've got over a sector of data to write, write it out with O_DIRECT
+ // and then continue writing the rest unaligned.
+ if (aligned && iovec_[write_index].iov_len > kSector) {
+ const size_t aligned_size =
+ iovec_[write_index].iov_len & (~(kSector - 1));
+ VLOG(1) << "Was aligned, writing last chunk rounded from "
+ << queue[i].size() << " to " << aligned_size;
+ iovec_[write_index].iov_len = aligned_size;
+
+ const auto code =
+ WriteV(iovec_.data(), write_index + 1, true, counted_size + aligned_size);
+ if (code == WriteCode::kOutOfSpace) {
+ return {
+ .code = code,
+ .messages_written = i,
+ };
+ }
+
+ // Now, everything before here has been written. Make an iovec out of
+ // the rest and keep going.
+ write_index = 0;
+ counted_size = 0;
+
+ iovec_[write_index].iov_base =
+ const_cast<uint8_t *>(queue[i].data() + aligned_size);
+ iovec_[write_index].iov_len = queue[i].size() - aligned_size;
+ }
+ aligned = false;
+ }
+ VLOG(1) << "Writing " << iovec_[write_index].iov_len << " to "
+ << filename();
+ counted_size += iovec_[write_index].iov_len;
+ ++write_index;
+ }
+
+ // Either write the aligned data if it is all aligned, or write the rest
+ // unaligned if we wrote aligned up above.
+ const auto code = WriteV(iovec_.data(), write_index, aligned, counted_size);
+ return {
+ .code = code,
+ .messages_written = queue.size(),
+ };
+}
+
+WriteCode FileHandler::WriteV(struct iovec *iovec_data, size_t iovec_size,
+ bool aligned, size_t counted_size) {
+ // Configure the file descriptor to match the mode we should be in. This is
+ // safe to over-call since it only does the syscall if needed.
+ if (aligned) {
+ EnableDirect();
+ } else {
+ DisableDirect();
+ }
+
+ const auto start = aos::monotonic_clock::now();
+
+ if (VLOG_IS_ON(2)) {
+ size_t to_be_written = 0;
+ for (size_t i = 0; i < iovec_size; ++i) {
+ VLOG(2) << " iov_base " << static_cast<void *>(iovec_data[i].iov_base)
+ << ", iov_len " << iovec_data[i].iov_len;
+ to_be_written += iovec_data[i].iov_len;
+ }
+ CHECK_GT(to_be_written, 0u);
+ VLOG(2) << "Going to write " << to_be_written;
+ }
+
+ const ssize_t written = writev(fd_, iovec_data, iovec_size);
+ VLOG(2) << "Wrote " << written << ", for iovec size " << iovec_size;
+
+ if (FLAGS_sync && written > 0) {
+ // Flush asynchronously and force the data out of the cache.
+ sync_file_range(fd_, total_write_bytes_, written, SYNC_FILE_RANGE_WRITE);
+ if (last_synced_bytes_ != 0) {
+ // Per Linus' recommendation online on how to do fast file IO, do a
+ // blocking flush of the previous write chunk, and then tell the kernel to
+ // drop the pages from the cache. This makes sure we can't get too far
+ // ahead.
+ sync_file_range(fd_, last_synced_bytes_,
+ total_write_bytes_ - last_synced_bytes_,
+ SYNC_FILE_RANGE_WAIT_BEFORE | SYNC_FILE_RANGE_WRITE |
+ SYNC_FILE_RANGE_WAIT_AFTER);
+ posix_fadvise(fd_, last_synced_bytes_,
+ total_write_bytes_ - last_synced_bytes_,
+ POSIX_FADV_DONTNEED);
+
+ last_synced_bytes_ = total_write_bytes_;
+ }
+ }
+
+ const auto end = aos::monotonic_clock::now();
+ if (written == -1 && errno == ENOSPC) {
+ return WriteCode::kOutOfSpace;
+ }
+ PCHECK(written >= 0) << ": write failed, got " << written;
+ if (written < static_cast<ssize_t>(counted_size)) {
+ // Sometimes this happens instead of ENOSPC. On a real filesystem, this
+ // never seems to happen in any other case. If we ever want to log to a
+ // socket, this will happen more often. However, until we get there, we'll
+ // just assume it means we ran out of space.
+ return WriteCode::kOutOfSpace;
+ }
+
+ total_write_bytes_ += written;
+ write_stats_.UpdateStats(end - start, written, iovec_size);
+ return WriteCode::kOk;
+}
+
+WriteCode FileHandler::Close() {
+ if (!is_open()) {
+ return WriteCode::kOk;
+ }
+ bool ran_out_of_space = false;
+ if (close(fd_) == -1) {
+ if (errno == ENOSPC) {
+ ran_out_of_space = true;
+ } else {
+ PLOG(ERROR) << "Closing log file failed";
+ }
+ }
+ fd_ = -1;
+ VLOG(1) << "Closed " << filename_;
+ return ran_out_of_space ? WriteCode::kOutOfSpace : WriteCode::kOk;
+}
+
+FileBackend::FileBackend(std::string_view base_name)
+ : base_name_(base_name), separator_(base_name_.back() == '/' ? "" : "_") {}
+
+std::unique_ptr<FileHandler> FileBackend::RequestFile(std::string_view id) {
+ const std::string filename = absl::StrCat(base_name_, separator_, id);
+ return std::make_unique<FileHandler>(filename);
+}
+
+RenamableFileBackend::RenamableFileBackend(std::string_view base_name)
+ : base_name_(base_name), separator_(base_name_.back() == '/' ? "" : "_") {}
+
+std::unique_ptr<FileHandler> RenamableFileBackend::RequestFile(
+ std::string_view id) {
+ const std::string filename =
+ absl::StrCat(base_name_, separator_, id, temp_suffix_);
+ return std::make_unique<RenamableFileHandler>(this, filename);
+}
+
+void RenamableFileBackend::EnableTempFiles() {
+ use_temp_files_ = true;
+ temp_suffix_ = kTempExtension;
+}
+
+bool RenamableFileBackend::RenameLogBase(std::string_view new_base_name) {
+ if (new_base_name == base_name_) {
+ return true;
+ }
+ CHECK(old_base_name_.empty())
+ << "Only one change of base_name is supported. Was: " << old_base_name_;
+
+ std::string current_directory = base_name_;
+ std::string new_directory(new_base_name);
+
+ auto current_path_split = current_directory.rfind("/");
+ CHECK(current_path_split != std::string::npos)
+ << "Could not find / in the current directory path";
+ auto new_path_split = new_directory.rfind("/");
+ CHECK(new_path_split != std::string::npos)
+ << "Could not find / in the new directory path";
+
+ CHECK(new_base_name.substr(new_path_split) ==
+ current_directory.substr(current_path_split))
+ << "Rename of file base from " << current_directory << " to "
+ << new_directory << " is not supported.";
+
+ current_directory.resize(current_path_split);
+ new_directory.resize(new_path_split);
+ DIR *dir = opendir(current_directory.c_str());
+ if (dir) {
+ closedir(dir);
+ const int result = rename(current_directory.c_str(), new_directory.c_str());
+ if (result != 0) {
+ PLOG(ERROR) << "Unable to rename " << current_directory << " to "
+ << new_directory;
+ return false;
+ }
+ } else {
+ // Handle if directory was already renamed.
+ dir = opendir(new_directory.c_str());
+ if (!dir) {
+ LOG(ERROR) << "Old directory " << current_directory
+ << " missing and new directory " << new_directory
+ << " not present.";
+ return false;
+ }
+ closedir(dir);
+ }
+ old_base_name_ = base_name_;
+ base_name_ = std::string(new_base_name);
+ separator_ = base_name_.back() == '/' ? "" : "_";
+ return true;
+}
+
+WriteCode RenamableFileBackend::RenameFileAfterClose(
+ std::string_view filename) {
+ // Fast check that we can skip rename.
+ if (!use_temp_files_ && old_base_name_.empty()) {
+ return WriteCode::kOk;
+ }
+
+ std::string current_filename(filename);
+
+ // When changing the base name, we rename the log folder while there active
+ // buffer writers. Therefore, the name of that active buffer may still refer
+ // to the old file location rather than the new one.
+ if (!old_base_name_.empty()) {
+ auto offset = current_filename.find(old_base_name_);
+ if (offset != std::string::npos) {
+ current_filename.replace(offset, old_base_name_.length(), base_name_);
+ }
+ }
+
+ std::string final_filename = current_filename;
+ if (use_temp_files_) {
+ CHECK(current_filename.size() > temp_suffix_.size());
+ final_filename = current_filename.substr(
+ 0, current_filename.size() - temp_suffix_.size());
+ }
+
+ int result = rename(current_filename.c_str(), final_filename.c_str());
+
+ bool ran_out_of_space = false;
+ if (result != 0) {
+ if (errno == ENOSPC) {
+ ran_out_of_space = true;
+ } else {
+ PLOG(FATAL) << "Renaming " << current_filename << " to " << final_filename
+ << " failed";
+ }
+ } else {
+ VLOG(1) << "Renamed " << current_filename << " -> " << final_filename;
+ }
+ return ran_out_of_space ? WriteCode::kOutOfSpace : WriteCode::kOk;
+}
+
+WriteCode RenamableFileBackend::RenamableFileHandler::Close() {
+ if (!is_open()) {
+ return WriteCode::kOk;
+ }
+ if (FileHandler::Close() == WriteCode::kOutOfSpace) {
+ return WriteCode::kOutOfSpace;
+ }
+ if (owner_->RenameFileAfterClose(filename()) == WriteCode::kOutOfSpace) {
+ return WriteCode::kOutOfSpace;
+ }
+ return WriteCode::kOk;
+}
+} // namespace aos::logger
diff --git a/aos/events/logging/log_backend.h b/aos/events/logging/log_backend.h
new file mode 100644
index 0000000..20ade91
--- /dev/null
+++ b/aos/events/logging/log_backend.h
@@ -0,0 +1,260 @@
+#ifndef AOS_EVENTS_LOGGING_LOG_BACKEND_H_
+#define AOS_EVENTS_LOGGING_LOG_BACKEND_H_
+
+#include <fcntl.h>
+#include <sys/types.h>
+#include <sys/uio.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "absl/types/span.h"
+#include "aos/time/time.h"
+
+namespace aos::logger {
+
+class WriteStats {
+ public:
+ // The maximum time for a single write call, or 0 if none have been performed.
+ std::chrono::nanoseconds max_write_time() const { return max_write_time_; }
+ // The number of bytes in the longest write call, or -1 if none have been
+ // performed.
+ int max_write_time_bytes() const { return max_write_time_bytes_; }
+ // The number of buffers in the longest write call, or -1 if none have been
+ // performed.
+ int max_write_time_messages() const { return max_write_time_messages_; }
+ // The total time spent in write calls.
+ std::chrono::nanoseconds total_write_time() const {
+ return total_write_time_;
+ }
+ // The total number of writes which have been performed.
+ int total_write_count() const { return total_write_count_; }
+ // The total number of messages which have been written.
+ int total_write_messages() const { return total_write_messages_; }
+ // The total number of bytes which have been written.
+ int total_write_bytes() const { return total_write_bytes_; }
+
+ void ResetStats() {
+ max_write_time_ = std::chrono::nanoseconds::zero();
+ max_write_time_bytes_ = -1;
+ max_write_time_messages_ = -1;
+ total_write_time_ = std::chrono::nanoseconds::zero();
+ total_write_count_ = 0;
+ total_write_messages_ = 0;
+ total_write_bytes_ = 0;
+ }
+
+ void UpdateStats(aos::monotonic_clock::duration duration, ssize_t written,
+ int iovec_size) {
+ if (duration > max_write_time_) {
+ max_write_time_ = duration;
+ max_write_time_bytes_ = written;
+ max_write_time_messages_ = iovec_size;
+ }
+ total_write_time_ += duration;
+ ++total_write_count_;
+ total_write_messages_ += iovec_size;
+ total_write_bytes_ += written;
+ }
+
+ private:
+ std::chrono::nanoseconds max_write_time_ = std::chrono::nanoseconds::zero();
+ int max_write_time_bytes_ = -1;
+ int max_write_time_messages_ = -1;
+ std::chrono::nanoseconds total_write_time_ = std::chrono::nanoseconds::zero();
+ int total_write_count_ = 0;
+ int total_write_messages_ = 0;
+ int total_write_bytes_ = 0;
+};
+
+// Currently, all write operations only cares about out-of-space error. This is
+// a simple representation of write result.
+enum class WriteCode { kOk, kOutOfSpace };
+
+struct WriteResult {
+ WriteCode code = WriteCode::kOk;
+ size_t messages_written = 0;
+};
+
+// FileHandler is a replacement for bare filename in log writing and reading
+// operations.
+//
+// There are a couple over-arching constraints on writing to keep track of.
+// 1) The kernel is both faster and more efficient at writing large, aligned
+// chunks with O_DIRECT set on the file. The alignment needed is specified
+// by kSector and is file system dependent.
+// 2) Not all encoders support generating round multiples of kSector of data.
+// Rather than burden the API for detecting when that is the case, we want
+// DetachedBufferWriter to be as efficient as it can at writing what given.
+// 3) Some files are small and not updated frequently. They need to be
+// flushed or we will lose data on power off. It is most efficient to write
+// as much as we can aligned by kSector and then fall back to the non direct
+// method when it has been flushed.
+// 4) Not all filesystems support O_DIRECT, and different sizes may be optimal
+// for different machines. The defaults should work decently anywhere and
+// be tunable for faster systems.
+// TODO (Alexei): need 2 variations, to support systems with and without
+// O_DIRECT
+class FileHandler {
+ public:
+ // Size of an aligned sector used to detect when the data is aligned enough to
+ // use O_DIRECT instead.
+ static constexpr size_t kSector = 512u;
+
+ explicit FileHandler(std::string filename);
+ virtual ~FileHandler();
+
+ FileHandler(const FileHandler &) = delete;
+ FileHandler &operator=(const FileHandler &) = delete;
+
+ // Try to open file. App will crash if there are other than out-of-space
+ // problems with backend media.
+ virtual WriteCode OpenForWrite();
+
+ // Close the file handler.
+ virtual WriteCode Close();
+
+ // This will be true until Close() is called, unless the file couldn't be
+ // created due to running out of space.
+ bool is_open() const { return fd_ != -1; }
+
+ // Peeks messages from queue and writes it to file. Returns code when
+ // out-of-space problem occurred along with number of messages from queue that
+ // was written.
+ //
+ // The spans can be aligned or not, and can have any lengths. This code will
+ // write faster if the spans passed in start at aligned addresses, and are
+ // multiples of kSector long (and the data written so far is also a multiple
+ // of kSector length).
+ virtual WriteResult Write(
+ const absl::Span<const absl::Span<const uint8_t>> &queue);
+
+ // TODO (Alexei): it is rather leaked abstraction.
+ // Path to the concrete log file.
+ std::string_view filename() const { return filename_; }
+
+ int fd() const { return fd_; }
+
+ // Get access to statistics related to the write operations.
+ WriteStats *WriteStatistics() { return &write_stats_; }
+
+ private:
+ // Enables O_DIRECT on the open file if it is supported. Cheap to call if it
+ // is already enabled.
+ void EnableDirect();
+ // Disables O_DIRECT on the open file if it is supported. Cheap to call if it
+ // is already disabld.
+ void DisableDirect();
+
+ bool ODirectEnabled() const { return !!(flags_ & O_DIRECT); }
+
+ // Writes a chunk of iovecs. aligned is true if all the data is kSector byte
+ // aligned and multiples of it in length, and counted_size is the sum of the
+ // sizes of all the chunks of data.
+ WriteCode WriteV(struct iovec *iovec_data, size_t iovec_size, bool aligned,
+ size_t counted_size);
+
+ const std::string filename_;
+
+ int fd_ = -1;
+
+ // List of iovecs to use with writev. This is a member variable to avoid
+ // churn.
+ std::vector<struct iovec> iovec_;
+
+ int total_write_bytes_ = 0;
+ int last_synced_bytes_ = 0;
+
+ bool supports_odirect_ = true;
+ int flags_ = 0;
+
+ WriteStats write_stats_;
+};
+
+// Class that decouples log writing and media (file system or memory). It is
+// handy to use for tests.
+class LogBackend {
+ public:
+ virtual ~LogBackend() = default;
+
+ // Request file-like object from the log backend. It maybe a file on a disk or
+ // in memory. id is usually generated by log namer and looks like name of the
+ // file within a log folder.
+ virtual std::unique_ptr<FileHandler> RequestFile(std::string_view id) = 0;
+};
+
+// Implements requests log files from file system.
+class FileBackend : public LogBackend {
+ public:
+ // base_name is the path to the folder where log files are.
+ explicit FileBackend(std::string_view base_name);
+ ~FileBackend() override = default;
+
+ // Request file from a file system. It is not open yet.
+ std::unique_ptr<FileHandler> RequestFile(std::string_view id) override;
+
+ private:
+ const std::string base_name_;
+ const std::string_view separator_;
+};
+
+// Provides a file backend that supports renaming of the base log folder and
+// temporary files.
+class RenamableFileBackend : public LogBackend {
+ public:
+ // Adds call to rename, when closed.
+ class RenamableFileHandler final : public FileHandler {
+ public:
+ RenamableFileHandler(RenamableFileBackend *owner, std::string filename)
+ : FileHandler(std::move(filename)), owner_(owner) {}
+ ~RenamableFileHandler() final = default;
+
+ // Returns false if not enough memory, true otherwise.
+ WriteCode Close() final;
+
+ private:
+ RenamableFileBackend *owner_;
+ };
+
+ explicit RenamableFileBackend(std::string_view base_name);
+ ~RenamableFileBackend() = default;
+
+ // Request file from a file system. It is not open yet.
+ std::unique_ptr<FileHandler> RequestFile(std::string_view id) override;
+
+ // TODO (Alexei): it is called by Logger, and left here for compatibility.
+ // Logger should not call it.
+ std::string_view base_name() { return base_name_; }
+
+ // If temp files are enabled, then this will write files with the .tmp
+ // suffix, and then rename them to the desired name after they are fully
+ // written.
+ //
+ // This is useful to enable incremental copying of the log files.
+ //
+ // Defaults to writing directly to the final filename.
+ void EnableTempFiles();
+
+ // Moves the current log location to the new name. Returns true if a change
+ // was made, false otherwise.
+ // Only renaming the folder is supported, not the file base name.
+ bool RenameLogBase(std::string_view new_base_name);
+
+ private:
+ // This function called after file closed, to adjust file names in case of
+ // base name was changed or temp files enabled.
+ WriteCode RenameFileAfterClose(std::string_view filename);
+
+ std::string base_name_;
+ std::string_view separator_;
+
+ bool use_temp_files_ = false;
+ std::string_view temp_suffix_;
+
+ std::string old_base_name_;
+};
+
+} // namespace aos::logger
+
+#endif // AOS_EVENTS_LOGGING_LOG_BACKEND_H_
diff --git a/aos/events/logging/log_backend_test.cc b/aos/events/logging/log_backend_test.cc
new file mode 100644
index 0000000..e940948
--- /dev/null
+++ b/aos/events/logging/log_backend_test.cc
@@ -0,0 +1,320 @@
+#include "aos/events/logging/log_backend.h"
+
+#include <algorithm>
+#include <filesystem>
+#include <fstream>
+#include <random>
+
+#include "absl/strings/str_cat.h"
+#include "absl/strings/str_join.h"
+#include "aos/containers/resizeable_buffer.h"
+#include "aos/testing/tmpdir.h"
+#include "glog/logging.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace aos::logger::testing {
+TEST(LogBackendTest, CreateSimpleFile) {
+ const std::string logevent = aos::testing::TestTmpDir() + "/logevent/";
+ FileBackend backend(logevent);
+ auto file = backend.RequestFile("test.log");
+ ASSERT_EQ(file->OpenForWrite(), WriteCode::kOk);
+ auto result = write(file->fd(), "test", 4);
+ EXPECT_GT(result, 0);
+ EXPECT_EQ(file->Close(), WriteCode::kOk);
+ EXPECT_TRUE(std::filesystem::exists(logevent + "test.log"));
+}
+
+TEST(LogBackendTest, CreateRenamableFile) {
+ const std::string logevent = aos::testing::TestTmpDir() + "/logevent/";
+ RenamableFileBackend backend(logevent);
+ auto file = backend.RequestFile("test.log");
+ ASSERT_EQ(file->OpenForWrite(), WriteCode::kOk);
+ auto result = write(file->fd(), "testtest", 8);
+ EXPECT_GT(result, 0);
+ EXPECT_EQ(file->Close(), WriteCode::kOk);
+ EXPECT_TRUE(std::filesystem::exists(logevent + "test.log"));
+}
+
+TEST(LogBackendTest, UseTempRenamableFile) {
+ const std::string logevent = aos::testing::TestTmpDir() + "/logevent/";
+ RenamableFileBackend backend(logevent);
+ backend.EnableTempFiles();
+ auto file = backend.RequestFile("test.log");
+ ASSERT_EQ(file->OpenForWrite(), WriteCode::kOk);
+ auto result = write(file->fd(), "testtest", 8);
+ EXPECT_GT(result, 0);
+ EXPECT_TRUE(std::filesystem::exists(logevent + "test.log.tmp"));
+
+ EXPECT_EQ(file->Close(), WriteCode::kOk);
+ // Check that file is renamed.
+ EXPECT_TRUE(std::filesystem::exists(logevent + "test.log"));
+}
+
+TEST(LogBackendTest, RenameBaseAfterWrite) {
+ const std::string logevent = aos::testing::TestTmpDir() + "/logevent/";
+ RenamableFileBackend backend(logevent);
+ auto file = backend.RequestFile("test.log");
+ ASSERT_EQ(file->OpenForWrite(), WriteCode::kOk);
+ auto result = write(file->fd(), "testtest", 8);
+ EXPECT_GT(result, 0);
+ EXPECT_TRUE(std::filesystem::exists(logevent + "test.log"));
+
+ std::string renamed = aos::testing::TestTmpDir() + "/renamed/";
+ backend.RenameLogBase(renamed);
+
+ EXPECT_FALSE(std::filesystem::exists(logevent + "test.log"));
+ EXPECT_TRUE(std::filesystem::exists(renamed + "test.log"));
+
+ EXPECT_EQ(file->Close(), WriteCode::kOk);
+ // Check that file is renamed.
+ EXPECT_TRUE(std::filesystem::exists(renamed + "test.log"));
+}
+
+TEST(LogBackendTest, UseTestAndRenameBaseAfterWrite) {
+ const std::string logevent = aos::testing::TestTmpDir() + "/logevent/";
+ RenamableFileBackend backend(logevent);
+ backend.EnableTempFiles();
+ auto file = backend.RequestFile("test.log");
+ ASSERT_EQ(file->OpenForWrite(), WriteCode::kOk);
+ auto result = write(file->fd(), "testtest", 8);
+ EXPECT_GT(result, 0);
+ EXPECT_TRUE(std::filesystem::exists(logevent + "test.log.tmp"));
+
+ std::string renamed = aos::testing::TestTmpDir() + "/renamed/";
+ backend.RenameLogBase(renamed);
+
+ EXPECT_FALSE(std::filesystem::exists(logevent + "test.log.tmp"));
+ EXPECT_TRUE(std::filesystem::exists(renamed + "test.log.tmp"));
+
+ EXPECT_EQ(file->Close(), WriteCode::kOk);
+ // Check that file is renamed.
+ EXPECT_TRUE(std::filesystem::exists(renamed + "test.log"));
+}
+
+// It represents calls to Write function (batching of calls and messages) where
+// int values are sizes of each message in the queue.
+using WriteRecipe = std::vector<std::vector<int>>;
+
+struct FileWriteTestBase : public ::testing::Test {
+ uint8_t NextRandom() { return distribution(engine); }
+
+ class AlignedReallocator {
+ public:
+ static void *Realloc(void *old, size_t old_size, size_t new_capacity) {
+ void *new_memory = std::aligned_alloc(512, new_capacity);
+ if (old) {
+ memcpy(new_memory, old, old_size);
+ free(old);
+ }
+ return new_memory;
+ }
+ };
+
+ AllocatorResizeableBuffer<AlignedReallocator> buffer;
+
+ void TestRecipe(const WriteRecipe &recipe) {
+ VLOG(1) << "Starting";
+ for (const std::vector<int> &r : recipe) {
+ VLOG(1) << " chunk " << absl::StrJoin(r, ", ");
+ }
+ size_t requested_size = 0;
+ for (const auto &call : recipe) {
+ for (const auto &message_size : call) {
+ requested_size += message_size;
+ }
+ }
+
+ // Grow the cached buffer if it needs to grow. Building a random buffer is
+ // the most expensive part of the test.
+ if (buffer.size() < requested_size) {
+ // Make sure it is 512 aligned... That makes sure we have the best chance
+ // of transitioning to and from being aligned.
+ buffer.resize((requested_size + FileHandler::kSector - 1) &
+ (~(FileHandler::kSector - 1)));
+ std::generate(std::begin(buffer), std::end(buffer),
+ [this]() { return NextRandom(); });
+ }
+
+ // Back align the data to the buffer so we make sure the contents of the
+ // file keep changing in case a file somehow doesn't get deleted, or
+ // collides with something else.
+ const uint8_t *adjusted_start =
+ buffer.data() + buffer.size() - requested_size;
+
+ // logevent has to end with '/' to be recognized as a folder.
+ const std::string logevent = aos::testing::TestTmpDir() + "/";
+ const auto file = std::filesystem::path(logevent) / "test.log";
+ std::filesystem::remove_all(file);
+ VLOG(1) << "Writing to " << file.c_str();
+
+ FileBackend backend(logevent);
+ auto handler = backend.RequestFile("test.log");
+ ASSERT_EQ(handler->OpenForWrite(), WriteCode::kOk);
+
+ // Build arguments for Write.
+ size_t position = 0;
+ for (const auto &call : recipe) {
+ std::vector<absl::Span<const uint8_t>> queue;
+ for (const auto &message_size : call) {
+ const uint8_t *current = adjusted_start + position;
+ queue.emplace_back(current, message_size);
+ position += message_size;
+ }
+ auto result = handler->Write(queue);
+ EXPECT_EQ(result.code, WriteCode::kOk);
+ EXPECT_EQ(result.messages_written, call.size());
+ }
+
+ ASSERT_EQ(handler->Close(), WriteCode::kOk);
+ EXPECT_TRUE(std::filesystem::exists(file));
+ EXPECT_EQ(std::filesystem::file_size(file), requested_size);
+
+ // Confirm that the contents then match the original buffer.
+ std::ifstream file_stream(file, std::ios::in | std::ios::binary);
+ std::vector<uint8_t> content((std::istreambuf_iterator<char>(file_stream)),
+ std::istreambuf_iterator<char>());
+ ASSERT_EQ(content.size(), requested_size);
+ bool matches = true;
+ for (size_t i = 0; i < content.size(); ++i) {
+ if (content[i] != adjusted_start[i]) {
+ matches = false;
+ break;
+ }
+ }
+ if (!matches) {
+ ASSERT_TRUE(false);
+ }
+ }
+
+ std::random_device random;
+ std::mt19937 engine{random()};
+ std::uniform_int_distribution<uint8_t> distribution{0, 0xFF};
+};
+
+// Tests that random sets of reads and writes always result in all the data
+// being written.
+TEST_F(FileWriteTestBase, RandomTest) {
+ std::mt19937 engine2{random()};
+ std::uniform_int_distribution<int> count_distribution{1, 5};
+
+ // Pick a bunch of lengths that will result in things that add up to multiples
+ // of 512 and end up transitioning across the aligned and unaligned boundary.
+ const std::vector<int> lengths = {
+ 0x100b5, 0xff4b, 0x10000, 1024 - 7, 1024 - 6, 1024 - 5, 1024 - 4,
+ 1024 - 3, 1024 - 2, 1024 - 1, 1024, 1024 + 1, 1024 + 2, 1024 + 3,
+ 1024 + 4, 1024 + 5, 1024 + 6, 1024 + 7, 512 - 7, 512 - 6, 512 - 5,
+ 512 - 4, 512 - 3, 512 - 2, 512 - 1, 512, 512 + 1, 512 + 2,
+ 512 + 3, 512 + 4, 512 + 5, 512 + 6, 512 + 7};
+ std::uniform_int_distribution<int> lengths_distribution{
+ 0, static_cast<int>(lengths.size() - 1)};
+
+ for (int i = 0; i < 100000; ++i) {
+ WriteRecipe recipe;
+ int number_of_writes = count_distribution(engine2);
+ for (int j = 0; j < number_of_writes; ++j) {
+ int number_of_chunks = count_distribution(engine2);
+ std::vector<int> r;
+ for (int k = 0; k < number_of_chunks; ++k) {
+ r.emplace_back(lengths[lengths_distribution(engine2)]);
+ }
+ recipe.emplace_back(std::move(r));
+ }
+
+ TestRecipe(recipe);
+ }
+}
+
+// Test an aligned to unaligned transition to make sure everything works.
+TEST_F(FileWriteTestBase, AlignedToUnaligned) {
+ AllocatorResizeableBuffer<AlignedReallocator> aligned_buffer;
+ AllocatorResizeableBuffer<AlignedReallocator> unaligned_buffer;
+
+ aligned_buffer.resize(FileHandler::kSector * 4);
+ std::generate(std::begin(aligned_buffer), std::end(aligned_buffer),
+ [this]() { return NextRandom(); });
+
+ unaligned_buffer.resize(FileHandler::kSector * 4);
+ std::generate(std::begin(unaligned_buffer), std::end(unaligned_buffer),
+ [this]() { return NextRandom(); });
+
+ const size_t kOffset = 53;
+ absl::Span<const uint8_t> unaligned_span(unaligned_buffer.data() + kOffset,
+ aligned_buffer.size() - kOffset);
+
+ std::vector<absl::Span<const uint8_t>> queue;
+
+ queue.emplace_back(aligned_buffer.data(), aligned_buffer.size());
+ queue.emplace_back(unaligned_span);
+ LOG(INFO) << "Queue 0 " << queue[0].size();
+ LOG(INFO) << "Queue 1 " << queue[1].size();
+
+ const std::string logevent = aos::testing::TestTmpDir() + "/";
+ const auto file = std::filesystem::path(logevent) / "test.log";
+ std::filesystem::remove_all(file);
+ VLOG(1) << "Writing to " << file.c_str();
+
+ FileBackend backend(logevent);
+ auto handler = backend.RequestFile("test.log");
+ ASSERT_EQ(handler->OpenForWrite(), WriteCode::kOk);
+
+ auto result = handler->Write(queue);
+ EXPECT_EQ(result.code, WriteCode::kOk);
+ EXPECT_EQ(result.messages_written, queue.size());
+
+ ASSERT_EQ(handler->Close(), WriteCode::kOk);
+ EXPECT_TRUE(std::filesystem::exists(file));
+ const size_t requested_size = queue[0].size() + queue[1].size();
+ EXPECT_EQ(std::filesystem::file_size(file), requested_size);
+
+ // Confirm that the contents then match the original buffer.
+ std::ifstream file_stream(file, std::ios::in | std::ios::binary);
+ std::vector<uint8_t> content((std::istreambuf_iterator<char>(file_stream)),
+ std::istreambuf_iterator<char>());
+ ASSERT_EQ(content.size(), requested_size);
+ bool matches = true;
+ for (size_t i = 0; i < queue[0].size(); ++i) {
+ if (content[i] != aligned_buffer.data()[i]) {
+ matches = false;
+ break;
+ }
+ }
+ for (size_t i = 0; i < queue[1].size(); ++i) {
+ if (content[i + queue[0].size()] != unaligned_span.data()[i]) {
+ matches = false;
+ break;
+ }
+ }
+ if (!matches) {
+ ASSERT_TRUE(false);
+ }
+}
+
+struct FileWriteTestFixture : public ::testing::WithParamInterface<WriteRecipe>,
+ public FileWriteTestBase {};
+
+TEST_P(FileWriteTestFixture, CheckSizeOfWrittenFile) {
+ auto recipe = GetParam();
+ TestRecipe(recipe);
+}
+
+// Try out some well known failure cases transitioning across the alignment
+// boundary.
+INSTANTIATE_TEST_SUITE_P(
+ FileWriteTest, FileWriteTestFixture,
+ ::testing::Values(WriteRecipe{{0x10000}}, WriteRecipe{{0x10000, 0x1000b5}},
+ WriteRecipe{{0x10000, 0x1000b5}, {0xfff4b, 0x10000}},
+ WriteRecipe{{0x1000b5, 0xfff4b}, {0x10000}},
+ WriteRecipe{{65536, 517, 65717}},
+ WriteRecipe{{65536, 517, 518, 511},
+ {514},
+ {505, 514},
+ {505, 514, 65355, 519}},
+ WriteRecipe{{65536, 518, 511, 511},
+ {65717},
+ {65717, 65717, 518},
+ {65536, 65536, 508, 65355},
+ {515, 519}},
+ WriteRecipe{{0x1000b5, 0xfff4b, 0x100000}, {0x10000}}));
+
+} // namespace aos::logger::testing
diff --git a/aos/events/logging/log_edit.cc b/aos/events/logging/log_edit.cc
index dca56bb..55b7666 100644
--- a/aos/events/logging/log_edit.cc
+++ b/aos/events/logging/log_edit.cc
@@ -48,7 +48,7 @@
aos::logger::SpanReader span_reader(orig_path);
CHECK(!span_reader.ReadMessage().empty()) << ": Empty header, aborting";
- aos::logger::DetachedBufferWriter buffer_writer(
+ aos::logger::DetachedBufferFileWriter buffer_writer(
FLAGS_logfile,
std::make_unique<aos::logger::DummyEncoder>(FLAGS_max_message_size));
{
diff --git a/aos/events/logging/log_namer.cc b/aos/events/logging/log_namer.cc
index 86d813f..8ec1e70 100644
--- a/aos/events/logging/log_namer.cc
+++ b/aos/events/logging/log_namer.cc
@@ -560,17 +560,16 @@
return result;
}
-MultiNodeLogNamer::MultiNodeLogNamer(std::string_view base_name,
- EventLoop *event_loop)
- : MultiNodeLogNamer(base_name, event_loop->configuration(), event_loop,
- event_loop->node()) {}
+MultiNodeLogNamer::MultiNodeLogNamer(
+ std::unique_ptr<RenamableFileBackend> log_backend, EventLoop *event_loop)
+ : MultiNodeLogNamer(std::move(log_backend), event_loop->configuration(),
+ event_loop, event_loop->node()) {}
-MultiNodeLogNamer::MultiNodeLogNamer(std::string_view base_name,
- const Configuration *configuration,
- EventLoop *event_loop, const Node *node)
+MultiNodeLogNamer::MultiNodeLogNamer(
+ std::unique_ptr<RenamableFileBackend> log_backend,
+ const Configuration *configuration, EventLoop *event_loop, const Node *node)
: LogNamer(configuration, event_loop, node),
- base_name_(base_name),
- old_base_name_(),
+ log_backend_(std::move(log_backend)),
encoder_factory_([](size_t max_message_size) {
// TODO(austin): For slow channels, can we allocate less memory?
return std::make_unique<DummyEncoder>(max_message_size,
@@ -606,21 +605,19 @@
return;
}
- const std::string_view separator = base_name_.back() == '/' ? "" : "_";
- const std::string filename = absl::StrCat(
- base_name_, separator, config_sha256, ".bfbs", extension_, temp_suffix_);
-
+ const std::string filename = absl::StrCat(config_sha256, ".bfbs", extension_);
+ auto file_handle = log_backend_->RequestFile(filename);
std::unique_ptr<DetachedBufferWriter> writer =
std::make_unique<DetachedBufferWriter>(
- filename, encoder_factory_(header->span().size()));
+ std::move(file_handle), encoder_factory_(header->span().size()));
DataEncoder::SpanCopier coppier(header->span());
writer->CopyMessage(&coppier, aos::monotonic_clock::now());
if (!writer->ran_out_of_space()) {
- all_filenames_.emplace_back(
- absl::StrCat(config_sha256, ".bfbs", extension_));
+ all_filenames_.emplace_back(filename);
}
+ // Close the file and maybe rename it too.
CloseWriter(&writer);
}
@@ -646,8 +643,8 @@
if (!data_writer_) {
MakeDataWriter();
}
- data_writer_->UpdateMaxMessageSize(PackMessageSize(
- LogType::kLogRemoteMessage, channel->max_size()));
+ data_writer_->UpdateMaxMessageSize(
+ PackMessageSize(LogType::kLogRemoteMessage, channel->max_size()));
return data_writer_.get();
}
@@ -726,10 +723,10 @@
for (std::pair<const Channel *const, NewDataWriter> &data_writer :
data_writers_) {
if (!data_writer.second.writer) continue;
- data_writer.second.writer->ResetStatistics();
+ data_writer.second.writer->WriteStatistics()->ResetStats();
}
if (data_writer_) {
- data_writer_->writer->ResetStatistics();
+ data_writer_->writer->WriteStatistics()->ResetStats();
}
max_write_time_ = std::chrono::nanoseconds::zero();
max_write_time_bytes_ = -1;
@@ -789,7 +786,7 @@
// Refuse to open any new files, which might skip data. Any existing files
// are in the same folder, which means they're on the same filesystem, which
// means they're probably going to run out of space and get stuck too.
- if (!destination->get()) {
+ if (!(*destination)) {
// But avoid leaving a nullptr writer if we're out of space when
// attempting to open the first file.
*destination = std::make_unique<DetachedBufferWriter>(
@@ -797,102 +794,54 @@
}
return;
}
- const std::string_view separator = base_name_.back() == '/' ? "" : "_";
- const std::string filename =
- absl::StrCat(base_name_, separator, path, temp_suffix_);
- if (!destination->get()) {
+
+ // Let's check that we need to close and replace current driver.
+ if (*destination) {
+ // Let's close the current writer.
+ CloseWriter(destination);
+ // Are we out of space now?
if (ran_out_of_space_) {
*destination = std::make_unique<DetachedBufferWriter>(
DetachedBufferWriter::already_out_of_space_t());
return;
}
- *destination = std::make_unique<DetachedBufferWriter>(
- filename, encoder_factory_(max_message_size));
- if (!destination->get()->ran_out_of_space()) {
- all_filenames_.emplace_back(path);
- }
- return;
}
- CloseWriter(destination);
- if (ran_out_of_space_) {
- *destination->get() =
- DetachedBufferWriter(DetachedBufferWriter::already_out_of_space_t());
- return;
- }
-
- *destination->get() =
- DetachedBufferWriter(filename, encoder_factory_(max_message_size));
- if (!destination->get()->ran_out_of_space()) {
+ const std::string filename(path);
+ *destination = std::make_unique<DetachedBufferWriter>(
+ log_backend_->RequestFile(filename), encoder_factory_(max_message_size));
+ if (!(*destination)->ran_out_of_space()) {
all_filenames_.emplace_back(path);
}
}
-void MultiNodeLogNamer::RenameTempFile(DetachedBufferWriter *destination) {
- if (temp_suffix_.empty()) {
- return;
- }
- std::string current_filename = std::string(destination->filename());
- CHECK(current_filename.size() > temp_suffix_.size());
- std::string final_filename =
- current_filename.substr(0, current_filename.size() - temp_suffix_.size());
- int result = rename(current_filename.c_str(), final_filename.c_str());
-
- // When changing the base name, we rename the log folder while there active
- // buffer writers. Therefore, the name of that active buffer may still refer
- // to the old file location rather than the new one. This minimized changes to
- // existing code.
- if (result != 0 && errno != ENOSPC && !old_base_name_.empty()) {
- auto offset = current_filename.find(old_base_name_);
- if (offset != std::string::npos) {
- current_filename.replace(offset, old_base_name_.length(), base_name_);
- }
- offset = final_filename.find(old_base_name_);
- if (offset != std::string::npos) {
- final_filename.replace(offset, old_base_name_.length(), base_name_);
- }
- result = rename(current_filename.c_str(), final_filename.c_str());
- }
-
- if (result != 0) {
- if (errno == ENOSPC) {
- ran_out_of_space_ = true;
- return;
- } else {
- PLOG(FATAL) << "Renaming " << current_filename << " to " << final_filename
- << " failed";
- }
- } else {
- VLOG(1) << "Renamed " << current_filename << " -> " << final_filename;
- }
-}
-
void MultiNodeLogNamer::CloseWriter(
std::unique_ptr<DetachedBufferWriter> *writer_pointer) {
- DetachedBufferWriter *const writer = writer_pointer->get();
- if (!writer) {
+ CHECK_NOTNULL(writer_pointer);
+ if (!(*writer_pointer)) {
return;
}
+ DetachedBufferWriter *const writer = writer_pointer->get();
const bool was_open = writer->is_open();
writer->Close();
- if (writer->max_write_time() > max_write_time_) {
- max_write_time_ = writer->max_write_time();
- max_write_time_bytes_ = writer->max_write_time_bytes();
- max_write_time_messages_ = writer->max_write_time_messages();
+ const auto *stats = writer->WriteStatistics();
+ if (stats->max_write_time() > max_write_time_) {
+ max_write_time_ = stats->max_write_time();
+ max_write_time_bytes_ = stats->max_write_time_bytes();
+ max_write_time_messages_ = stats->max_write_time_messages();
}
- total_write_time_ += writer->total_write_time();
- total_write_count_ += writer->total_write_count();
- total_write_messages_ += writer->total_write_messages();
- total_write_bytes_ += writer->total_write_bytes();
+ total_write_time_ += stats->total_write_time();
+ total_write_count_ += stats->total_write_count();
+ total_write_messages_ += stats->total_write_messages();
+ total_write_bytes_ += stats->total_write_bytes();
if (writer->ran_out_of_space()) {
ran_out_of_space_ = true;
writer->acknowledge_out_of_space();
}
- if (was_open) {
- RenameTempFile(writer);
- } else {
+
+ if (!was_open) {
CHECK(access(std::string(writer->filename()).c_str(), F_OK) == -1)
<< ": File should not exist: " << writer->filename();
}
diff --git a/aos/events/logging/log_namer.h b/aos/events/logging/log_namer.h
index 07edeac..3b54025 100644
--- a/aos/events/logging/log_namer.h
+++ b/aos/events/logging/log_namer.h
@@ -174,7 +174,7 @@
logger_node_index_(configuration::GetNodeIndex(configuration_, node_)) {
nodes_.emplace_back(node_);
}
- virtual ~LogNamer() {}
+ virtual ~LogNamer() = default;
virtual std::string_view base_name() const = 0;
@@ -183,6 +183,8 @@
// Rotate is called by Logger::RenameLogBase, which is currently the only user
// of this method.
// Only renaming the folder is supported, not the file base name.
+ // TODO (Alexei): it should not be in interface, since it is not applied to
+ // files.
virtual void set_base_name(std::string_view base_name) = 0;
// Returns a writer for writing data from messages on this channel (on the
@@ -297,32 +299,30 @@
aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader>::Empty();
};
-// Log namer which uses a config and a base name to name a bunch of files.
+// Log namer which uses a config to name a bunch of files.
class MultiNodeLogNamer : public LogNamer {
public:
- MultiNodeLogNamer(std::string_view base_name, EventLoop *event_loop);
- MultiNodeLogNamer(std::string_view base_name,
+ MultiNodeLogNamer(std::unique_ptr<RenamableFileBackend> log_backend,
+ EventLoop *event_loop);
+ MultiNodeLogNamer(std::unique_ptr<RenamableFileBackend> log_backend,
const Configuration *configuration, EventLoop *event_loop,
const Node *node);
~MultiNodeLogNamer() override;
- std::string_view base_name() const final { return base_name_; }
+ std::string_view base_name() const final { return log_backend_->base_name(); }
void set_base_name(std::string_view base_name) final {
- old_base_name_ = base_name_;
- base_name_ = base_name;
+ log_backend_->RenameLogBase(base_name);
}
- // If temp_suffix is set, then this will write files under names beginning
- // with the specified suffix, and then rename them to the desired name after
+ // When enabled, this will write files under names beginning
+ // with the .tmp suffix, and then rename them to the desired name after
// they are fully written.
//
// This is useful to enable incremental copying of the log files.
//
// Defaults to writing directly to the final filename.
- void set_temp_suffix(std::string_view temp_suffix) {
- temp_suffix_ = temp_suffix;
- }
+ void EnableTempFiles() { log_backend_->EnableTempFiles(); }
// Sets the function for creating encoders. The argument is the max message
// size (including headers) that will be written into this encoder.
@@ -396,7 +396,8 @@
return accumulate_data_writers(
max_write_time_,
[](std::chrono::nanoseconds x, const NewDataWriter &data_writer) {
- return std::max(x, data_writer.writer->max_write_time());
+ return std::max(
+ x, data_writer.writer->WriteStatistics()->max_write_time());
});
}
int max_write_time_bytes() const {
@@ -404,9 +405,11 @@
std::make_tuple(max_write_time_bytes_, max_write_time_),
[](std::tuple<int, std::chrono::nanoseconds> x,
const NewDataWriter &data_writer) {
- if (data_writer.writer->max_write_time() > std::get<1>(x)) {
- return std::make_tuple(data_writer.writer->max_write_time_bytes(),
- data_writer.writer->max_write_time());
+ if (data_writer.writer->WriteStatistics()->max_write_time() >
+ std::get<1>(x)) {
+ return std::make_tuple(
+ data_writer.writer->WriteStatistics()->max_write_time_bytes(),
+ data_writer.writer->WriteStatistics()->max_write_time());
}
return x;
}));
@@ -416,10 +419,12 @@
std::make_tuple(max_write_time_messages_, max_write_time_),
[](std::tuple<int, std::chrono::nanoseconds> x,
const NewDataWriter &data_writer) {
- if (data_writer.writer->max_write_time() > std::get<1>(x)) {
+ if (data_writer.writer->WriteStatistics()->max_write_time() >
+ std::get<1>(x)) {
return std::make_tuple(
- data_writer.writer->max_write_time_messages(),
- data_writer.writer->max_write_time());
+ data_writer.writer->WriteStatistics()
+ ->max_write_time_messages(),
+ data_writer.writer->WriteStatistics()->max_write_time());
}
return x;
}));
@@ -428,25 +433,26 @@
return accumulate_data_writers(
total_write_time_,
[](std::chrono::nanoseconds x, const NewDataWriter &data_writer) {
- return x + data_writer.writer->total_write_time();
+ return x + data_writer.writer->WriteStatistics()->total_write_time();
});
}
int total_write_count() const {
return accumulate_data_writers(
total_write_count_, [](int x, const NewDataWriter &data_writer) {
- return x + data_writer.writer->total_write_count();
+ return x + data_writer.writer->WriteStatistics()->total_write_count();
});
}
int total_write_messages() const {
return accumulate_data_writers(
total_write_messages_, [](int x, const NewDataWriter &data_writer) {
- return x + data_writer.writer->total_write_messages();
+ return x +
+ data_writer.writer->WriteStatistics()->total_write_messages();
});
}
int total_write_bytes() const {
return accumulate_data_writers(
total_write_bytes_, [](int x, const NewDataWriter &data_writer) {
- return x + data_writer.writer->total_write_bytes();
+ return x + data_writer.writer->WriteStatistics()->total_write_bytes();
});
}
@@ -466,8 +472,6 @@
void CreateBufferWriter(std::string_view path, size_t max_message_size,
std::unique_ptr<DetachedBufferWriter> *destination);
- void RenameTempFile(DetachedBufferWriter *destination);
-
void CloseWriter(std::unique_ptr<DetachedBufferWriter> *writer_pointer);
// A version of std::accumulate which operates over all of our DataWriters.
@@ -484,13 +488,11 @@
return t;
}
- std::string base_name_;
- std::string old_base_name_;
+ std::unique_ptr<RenamableFileBackend> log_backend_;
bool ran_out_of_space_ = false;
std::vector<std::string> all_filenames_;
- std::string temp_suffix_;
std::function<std::unique_ptr<DataEncoder>(size_t)> encoder_factory_;
std::string extension_;
@@ -509,6 +511,21 @@
std::map<const Channel *, NewDataWriter> data_writers_;
};
+// This is specialized log namer that deals with directory centric log events.
+class MultiNodeFilesLogNamer : public MultiNodeLogNamer {
+ public:
+ MultiNodeFilesLogNamer(std::string_view base_name, EventLoop *event_loop)
+ : MultiNodeLogNamer(std::make_unique<RenamableFileBackend>(base_name),
+ event_loop) {}
+
+ MultiNodeFilesLogNamer(std::string_view base_name,
+ const Configuration *configuration,
+ EventLoop *event_loop, const Node *node)
+ : MultiNodeLogNamer(std::make_unique<RenamableFileBackend>(base_name),
+ configuration, event_loop, node) {}
+ ~MultiNodeFilesLogNamer() override = default;
+};
+
} // namespace logger
} // namespace aos
diff --git a/aos/events/logging/log_writer.cc b/aos/events/logging/log_writer.cc
index 84f7503..1a333e7 100644
--- a/aos/events/logging/log_writer.cc
+++ b/aos/events/logging/log_writer.cc
@@ -236,44 +236,6 @@
if (new_base_name == CHECK_NOTNULL(log_namer_)->base_name()) {
return true;
}
- std::string current_directory = std::string(log_namer_->base_name());
- std::string new_directory = new_base_name;
-
- auto current_path_split = current_directory.rfind("/");
- CHECK(current_path_split != std::string::npos)
- << "Could not find / in the current directory path";
- auto new_path_split = new_directory.rfind("/");
- CHECK(new_path_split != std::string::npos)
- << "Could not find / in the new directory path";
-
- CHECK(new_base_name.substr(new_path_split) ==
- current_directory.substr(current_path_split))
- << "Rename of file base from " << current_directory << " to "
- << new_directory << " is not supported.";
-
- current_directory.resize(current_path_split);
- new_directory.resize(new_path_split);
- DIR *dir = opendir(current_directory.c_str());
- if (dir) {
- closedir(dir);
- const int result = rename(current_directory.c_str(), new_directory.c_str());
- if (result != 0) {
- PLOG(ERROR) << "Unable to rename " << current_directory << " to "
- << new_directory;
- return false;
- }
- } else {
- // Handle if directory was already renamed.
- dir = opendir(new_directory.c_str());
- if (!dir) {
- LOG(ERROR) << "Old directory " << current_directory
- << " missing and new directory " << new_directory
- << " not present.";
- return false;
- }
- closedir(dir);
- }
-
log_namer_->set_base_name(new_base_name);
Rotate();
return true;
diff --git a/aos/events/logging/log_writer.h b/aos/events/logging/log_writer.h
index 3e1832e..0063c9b 100644
--- a/aos/events/logging/log_writer.h
+++ b/aos/events/logging/log_writer.h
@@ -156,11 +156,11 @@
// Returns whether a log is currently being written.
bool is_started() const { return static_cast<bool>(log_namer_); }
- // Shortcut to call StartLogging with a MultiNodeLogNamer when event
+ // Shortcut to call StartLogging with a MultiNodeFilesLogNamer when event
// processing starts.
void StartLoggingOnRun(std::string base_name) {
event_loop_->OnRun([this, base_name]() {
- StartLogging(std::make_unique<MultiNodeLogNamer>(
+ StartLogging(std::make_unique<MultiNodeFilesLogNamer>(
base_name, configuration_, event_loop_, node_));
});
}
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 02a9fdd..990e742 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -7,6 +7,7 @@
#include <algorithm>
#include <climits>
+#include <filesystem>
#include "absl/strings/escaping.h"
#include "aos/configuration.h"
@@ -64,10 +65,6 @@
"corrupt message found by MessageReader be silently ignored, "
"providing access to all uncorrupted messages in a logfile.");
-DEFINE_bool(direct, false,
- "If true, write using O_DIRECT and write 512 byte aligned blocks "
- "whenever possible.");
-
namespace aos::logger {
namespace {
@@ -83,51 +80,14 @@
}
} // namespace
-DetachedBufferWriter::DetachedBufferWriter(std::string_view filename,
- std::unique_ptr<DataEncoder> encoder)
- : filename_(filename),
- encoder_(std::move(encoder)),
- supports_odirect_(FLAGS_direct) {
- iovec_.reserve(10);
- if (!util::MkdirPIfSpace(filename, 0777)) {
- ran_out_of_space_ = true;
- } else {
- fd_ = open(filename_.c_str(), O_RDWR | O_CLOEXEC | O_CREAT | O_EXCL, 0774);
- if (fd_ == -1 && errno == ENOSPC) {
- ran_out_of_space_ = true;
- } else {
- PCHECK(fd_ != -1) << ": Failed to open " << this->filename()
- << " for writing";
- VLOG(1) << "Opened " << this->filename() << " for writing";
-
- flags_ = fcntl(fd_, F_GETFL, 0);
- PCHECK(flags_ >= 0) << ": Failed to get flags for " << this->filename();
-
- EnableDirect();
- }
- }
-}
-
-void DetachedBufferWriter::EnableDirect() {
- if (supports_odirect_ && !ODirectEnabled()) {
- const int new_flags = flags_ | O_DIRECT;
- // Track if we failed to set O_DIRECT. Note: Austin hasn't seen this call
- // fail. The write call tends to fail instead.
- if (fcntl(fd_, F_SETFL, new_flags) == -1) {
- PLOG(WARNING) << "Failed to set O_DIRECT on " << filename();
- supports_odirect_ = false;
- } else {
- VLOG(1) << "Enabled O_DIRECT on " << filename();
- flags_ = new_flags;
- }
- }
-}
-
-void DetachedBufferWriter::DisableDirect() {
- if (supports_odirect_ && ODirectEnabled()) {
- flags_ = flags_ & (~O_DIRECT);
- PCHECK(fcntl(fd_, F_SETFL, flags_) != -1) << ": Failed to disable O_DIRECT";
- VLOG(1) << "Disabled O_DIRECT on " << filename();
+DetachedBufferWriter::DetachedBufferWriter(
+ std::unique_ptr<FileHandler> file_handler,
+ std::unique_ptr<DataEncoder> encoder)
+ : file_handler_(std::move(file_handler)), encoder_(std::move(encoder)) {
+ CHECK(file_handler_);
+ ran_out_of_space_ = file_handler_->OpenForWrite() == WriteCode::kOutOfSpace;
+ if (ran_out_of_space_) {
+ LOG(WARNING) << "And we are out of space";
}
}
@@ -148,22 +108,10 @@
// (because that data will then be its data).
DetachedBufferWriter &DetachedBufferWriter::operator=(
DetachedBufferWriter &&other) {
- std::swap(filename_, other.filename_);
+ std::swap(file_handler_, other.file_handler_);
std::swap(encoder_, other.encoder_);
- std::swap(fd_, other.fd_);
std::swap(ran_out_of_space_, other.ran_out_of_space_);
std::swap(acknowledge_ran_out_of_space_, other.acknowledge_ran_out_of_space_);
- std::swap(iovec_, other.iovec_);
- std::swap(max_write_time_, other.max_write_time_);
- std::swap(max_write_time_bytes_, other.max_write_time_bytes_);
- std::swap(max_write_time_messages_, other.max_write_time_messages_);
- std::swap(total_write_time_, other.total_write_time_);
- std::swap(total_write_count_, other.total_write_count_);
- std::swap(total_write_messages_, other.total_write_messages_);
- std::swap(total_write_bytes_, other.total_write_bytes_);
- std::swap(last_synced_bytes_, other.last_synced_bytes_);
- std::swap(supports_odirect_, other.supports_odirect_);
- std::swap(flags_, other.flags_);
std::swap(last_flush_time_, other.last_flush_time_);
return *this;
}
@@ -183,7 +131,8 @@
// Keep writing chunks until we've written it all. If we end up with a
// partial write, this means we need to flush to disk.
do {
- const size_t bytes_written = encoder_->Encode(copier, overall_bytes_written);
+ const size_t bytes_written =
+ encoder_->Encode(copier, overall_bytes_written);
CHECK(bytes_written != 0);
overall_bytes_written += bytes_written;
@@ -198,22 +147,14 @@
}
void DetachedBufferWriter::Close() {
- if (fd_ == -1) {
+ if (!file_handler_->is_open()) {
return;
}
encoder_->Finish();
while (encoder_->queue_size() > 0) {
Flush(monotonic_clock::max_time);
}
- if (close(fd_) == -1) {
- if (errno == ENOSPC) {
- ran_out_of_space_ = true;
- } else {
- PLOG(ERROR) << "Closing log file failed";
- }
- }
- fd_ = -1;
- VLOG(1) << "Closed " << filename();
+ ran_out_of_space_ = file_handler_->Close() == WriteCode::kOutOfSpace;
}
void DetachedBufferWriter::Flush(aos::monotonic_clock::time_point now) {
@@ -236,141 +177,9 @@
return;
}
- iovec_.clear();
- size_t iovec_size = std::min<size_t>(queue.size(), IOV_MAX);
- iovec_.resize(iovec_size);
- size_t counted_size = 0;
-
- // Ok, we now need to figure out if we were aligned, and if we were, how much
- // of the data we are being asked to write is aligned.
- //
- // The file is aligned if it is a multiple of kSector in length. The data is
- // aligned if it's memory is kSector aligned, and the length is a multiple of
- // kSector in length.
- bool aligned = (total_write_bytes_ % kSector) == 0;
- size_t write_index = 0;
- for (size_t i = 0; i < iovec_size; ++i) {
- iovec_[write_index].iov_base = const_cast<uint8_t *>(queue[i].data());
-
- // Make sure the address is aligned, or give up. This should be uncommon,
- // but is always possible.
- if ((reinterpret_cast<size_t>(iovec_[write_index].iov_base) &
- (kSector - 1)) != 0) {
- aligned = false;
- }
-
- // Now, see if the length is a multiple of kSector. The goal is to figure
- // out if/how much memory we can write out with O_DIRECT so that only the
- // last little bit is done with non-direct IO to keep it fast.
- iovec_[write_index].iov_len = queue[i].size();
- if ((iovec_[write_index].iov_len % kSector) != 0) {
- VLOG(1) << "Unaligned length on " << filename();
- // If we've got over a sector of data to write, write it out with O_DIRECT
- // and then continue writing the rest unaligned.
- if (aligned && iovec_[write_index].iov_len > kSector) {
- const size_t aligned_size =
- iovec_[write_index].iov_len & (~(kSector - 1));
- VLOG(1) << "Was aligned, writing last chunk rounded from "
- << queue[i].size() << " to " << aligned_size;
- iovec_[write_index].iov_len = aligned_size;
-
- WriteV(iovec_.data(), i + 1, true, counted_size + aligned_size);
-
- // Now, everything before here has been written. Make an iovec out of
- // the last bytes, and keep going.
- iovec_size -= write_index;
- iovec_.resize(iovec_size);
- write_index = 0;
- counted_size = 0;
-
- iovec_[write_index].iov_base =
- const_cast<uint8_t *>(queue[i].data() + aligned_size);
- iovec_[write_index].iov_len = queue[i].size() - aligned_size;
- }
- aligned = false;
- }
- VLOG(1) << "Writing " << iovec_[write_index].iov_len << " to "
- << filename();
- counted_size += iovec_[write_index].iov_len;
- ++write_index;
- }
-
- // Either write the aligned data if it is all aligned, or write the rest
- // unaligned if we wrote aligned up above.
- WriteV(iovec_.data(), iovec_.size(), aligned, counted_size);
-
- encoder_->Clear(iovec_size);
-}
-
-size_t DetachedBufferWriter::WriteV(struct iovec *iovec_data, size_t iovec_size,
- bool aligned, size_t counted_size) {
- // Configure the file descriptor to match the mode we should be in. This is
- // safe to over-call since it only does the syscall if needed.
- if (aligned) {
- EnableDirect();
- } else {
- DisableDirect();
- }
-
- const auto start = aos::monotonic_clock::now();
- const ssize_t written = writev(fd_, iovec_data, iovec_size);
-
- if (written > 0) {
- // Flush asynchronously and force the data out of the cache.
- sync_file_range(fd_, total_write_bytes_, written, SYNC_FILE_RANGE_WRITE);
- if (last_synced_bytes_ != 0) {
- // Per Linus' recommendation online on how to do fast file IO, do a
- // blocking flush of the previous write chunk, and then tell the kernel to
- // drop the pages from the cache. This makes sure we can't get too far
- // ahead.
- sync_file_range(fd_, last_synced_bytes_,
- total_write_bytes_ - last_synced_bytes_,
- SYNC_FILE_RANGE_WAIT_BEFORE | SYNC_FILE_RANGE_WRITE |
- SYNC_FILE_RANGE_WAIT_AFTER);
- posix_fadvise(fd_, last_synced_bytes_,
- total_write_bytes_ - last_synced_bytes_,
- POSIX_FADV_DONTNEED);
-
- last_synced_bytes_ = total_write_bytes_;
- }
- }
-
- const auto end = aos::monotonic_clock::now();
- HandleWriteReturn(written, counted_size);
-
- UpdateStatsForWrite(end - start, written, iovec_size);
-
- return written;
-}
-
-void DetachedBufferWriter::HandleWriteReturn(ssize_t write_return,
- size_t write_size) {
- if (write_return == -1 && errno == ENOSPC) {
- ran_out_of_space_ = true;
- return;
- }
- PCHECK(write_return >= 0) << ": write failed, got " << write_return;
- if (write_return < static_cast<ssize_t>(write_size)) {
- // Sometimes this happens instead of ENOSPC. On a real filesystem, this
- // never seems to happen in any other case. If we ever want to log to a
- // socket, this will happen more often. However, until we get there, we'll
- // just assume it means we ran out of space.
- ran_out_of_space_ = true;
- return;
- }
-}
-
-void DetachedBufferWriter::UpdateStatsForWrite(
- aos::monotonic_clock::duration duration, ssize_t written, int iovec_size) {
- if (duration > max_write_time_) {
- max_write_time_ = duration;
- max_write_time_bytes_ = written;
- max_write_time_messages_ = iovec_size;
- }
- total_write_time_ += duration;
- ++total_write_count_;
- total_write_messages_ += iovec_size;
- total_write_bytes_ += written;
+ const WriteResult result = file_handler_->Write(queue);
+ encoder_->Clear(result.messages_written);
+ ran_out_of_space_ = result.code == WriteCode::kOutOfSpace;
}
void DetachedBufferWriter::FlushAtThreshold(
@@ -401,10 +210,12 @@
while (encoder_->space() == 0 ||
encoder_->queued_bytes() > static_cast<size_t>(FLAGS_flush_size) ||
encoder_->queue_size() >= IOV_MAX ||
- now > last_flush_time_ +
- chrono::duration_cast<chrono::nanoseconds>(
- chrono::duration<double>(FLAGS_flush_period))) {
- VLOG(1) << "Chose to flush at " << now << ", last " << last_flush_time_;
+ (now > last_flush_time_ +
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(FLAGS_flush_period)) &&
+ encoder_->queued_bytes() != 0)) {
+ VLOG(1) << "Chose to flush at " << now << ", last " << last_flush_time_
+ << " queued bytes " << encoder_->queued_bytes();
Flush(now);
}
}
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 50f6b40..c75c8fb 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -20,6 +20,7 @@
#include "aos/events/event_loop.h"
#include "aos/events/logging/boot_timestamp.h"
#include "aos/events/logging/buffer_encoder.h"
+#include "aos/events/logging/log_backend.h"
#include "aos/events/logging/logfile_sorting.h"
#include "aos/events/logging/logger_generated.h"
#include "aos/flatbuffers.h"
@@ -44,31 +45,13 @@
// This class manages efficiently writing a sequence of detached buffers to a
// file. It encodes them, queues them up, and batches the write operation.
-//
-// There are a couple over-arching constraints on writing to keep track of.
-// 1) The kernel is both faster and more efficient at writing large, aligned
-// chunks with O_DIRECT set on the file. The alignment needed is specified
-// by kSector and is file system dependent.
-// 2) Not all encoders support generating round multiples of kSector of data.
-// Rather than burden the API for detecting when that is the case, we want
-// DetachedBufferWriter to be as efficient as it can at writing what given.
-// 3) Some files are small and not updated frequently. They need to be
-// flushed or we will lose data on power off. It is most efficient to write
-// as much as we can aligned by kSector and then fall back to the non direct
-// method when it has been flushed.
-// 4) Not all filesystems support O_DIRECT, and different sizes may be optimal
-// for different machines. The defaults should work decently anywhere and
-// be tuneable for faster systems.
+
class DetachedBufferWriter {
public:
// Marker struct for one of our constructor overloads.
struct already_out_of_space_t {};
- // Size of an aligned sector used to detect when the data is aligned enough to
- // use O_DIRECT instead.
- static constexpr size_t kSector = 512u;
-
- DetachedBufferWriter(std::string_view filename,
+ DetachedBufferWriter(std::unique_ptr<FileHandler> file_handler,
std::unique_ptr<DataEncoder> encoder);
// Creates a dummy instance which won't even open a file. It will act as if
// opening the file ran out of space immediately.
@@ -81,11 +64,11 @@
DetachedBufferWriter &operator=(DetachedBufferWriter &&other);
DetachedBufferWriter &operator=(const DetachedBufferWriter &) = delete;
- std::string_view filename() const { return filename_; }
+ std::string_view filename() const { return file_handler_->filename(); }
// This will be true until Close() is called, unless the file couldn't be
// created due to running out of space.
- bool is_open() const { return fd_ != -1; }
+ bool is_open() const { return file_handler_->is_open(); }
// Queues up a finished FlatBufferBuilder to be encoded and written.
//
@@ -123,33 +106,7 @@
return encoder_->total_bytes();
}
- // The maximum time for a single write call, or 0 if none have been performed.
- std::chrono::nanoseconds max_write_time() const { return max_write_time_; }
- // The number of bytes in the longest write call, or -1 if none have been
- // performed.
- int max_write_time_bytes() const { return max_write_time_bytes_; }
- // The number of buffers in the longest write call, or -1 if none have been
- // performed.
- int max_write_time_messages() const { return max_write_time_messages_; }
- // The total time spent in write calls.
- std::chrono::nanoseconds total_write_time() const {
- return total_write_time_;
- }
- // The total number of writes which have been performed.
- int total_write_count() const { return total_write_count_; }
- // The total number of messages which have been written.
- int total_write_messages() const { return total_write_messages_; }
- // The total number of bytes which have been written.
- int total_write_bytes() const { return total_write_bytes_; }
- void ResetStatistics() {
- max_write_time_ = std::chrono::nanoseconds::zero();
- max_write_time_bytes_ = -1;
- max_write_time_messages_ = -1;
- total_write_time_ = std::chrono::nanoseconds::zero();
- total_write_count_ = 0;
- total_write_messages_ = 0;
- total_write_bytes_ = 0;
- }
+ WriteStats* WriteStatistics() const { return file_handler_->WriteStatistics(); }
private:
// Performs a single writev call with as much of the data we have queued up as
@@ -161,61 +118,33 @@
// all of it.
void Flush(aos::monotonic_clock::time_point now);
- // write_return is what write(2) or writev(2) returned. write_size is the
- // number of bytes we expected it to write.
- void HandleWriteReturn(ssize_t write_return, size_t write_size);
-
- void UpdateStatsForWrite(aos::monotonic_clock::duration duration,
- ssize_t written, int iovec_size);
-
// Flushes data if we've reached the threshold to do that as part of normal
// operation either due to the outstanding queued data, or because we have
// passed our flush period. now is the current time to save some CPU grabbing
// the current time. It just needs to be close.
void FlushAtThreshold(aos::monotonic_clock::time_point now);
- // Enables O_DIRECT on the open file if it is supported. Cheap to call if it
- // is already enabled.
- void EnableDirect();
- // Disables O_DIRECT on the open file if it is supported. Cheap to call if it
- // is already disabld.
- void DisableDirect();
-
- // Writes a chunk of iovecs. aligned is true if all the data is kSector byte
- // aligned and multiples of it in length, and counted_size is the sum of the
- // sizes of all the chunks of data. Returns the size of data written.
- size_t WriteV(struct iovec *iovec_data, size_t iovec_size, bool aligned,
- size_t counted_size);
-
- bool ODirectEnabled() { return !!(flags_ & O_DIRECT); }
-
- std::string filename_;
+ std::unique_ptr<FileHandler> file_handler_;
std::unique_ptr<DataEncoder> encoder_;
- int fd_ = -1;
bool ran_out_of_space_ = false;
bool acknowledge_ran_out_of_space_ = false;
- // List of iovecs to use with writev. This is a member variable to avoid
- // churn.
- std::vector<struct iovec> iovec_;
-
- std::chrono::nanoseconds max_write_time_ = std::chrono::nanoseconds::zero();
- int max_write_time_bytes_ = -1;
- int max_write_time_messages_ = -1;
- std::chrono::nanoseconds total_write_time_ = std::chrono::nanoseconds::zero();
- int total_write_count_ = 0;
- int total_write_messages_ = 0;
- int total_write_bytes_ = 0;
- int last_synced_bytes_ = 0;
-
- bool supports_odirect_ = true;
- int flags_ = 0;
-
aos::monotonic_clock::time_point last_flush_time_ =
aos::monotonic_clock::min_time;
};
+// Specialized writer to single file
+class DetachedBufferFileWriter : public FileBackend,
+ public DetachedBufferWriter {
+ public:
+ DetachedBufferFileWriter(std::string_view filename,
+ std::unique_ptr<DataEncoder> encoder)
+ : FileBackend("/"),
+ DetachedBufferWriter(FileBackend::RequestFile(filename),
+ std::move(encoder)) {}
+};
+
// Repacks the provided RemoteMessage into fbb.
flatbuffers::Offset<MessageHeader> PackRemoteMessage(
flatbuffers::FlatBufferBuilder *fbb,
diff --git a/aos/events/logging/logfile_utils_out_of_space_test_runner.cc b/aos/events/logging/logfile_utils_out_of_space_test_runner.cc
index 719eb6e..03bfd0a 100644
--- a/aos/events/logging/logfile_utils_out_of_space_test_runner.cc
+++ b/aos/events/logging/logfile_utils_out_of_space_test_runner.cc
@@ -18,7 +18,7 @@
std::array<uint8_t, 10240> data;
data.fill(0);
- aos::logger::DetachedBufferWriter writer(
+ aos::logger::DetachedBufferFileWriter writer(
FLAGS_tmpfs + "/file",
std::make_unique<aos::logger::DummyEncoder>(data.size()));
for (int i = 0; i < 8; ++i) {
diff --git a/aos/events/logging/logfile_utils_test.cc b/aos/events/logging/logfile_utils_test.cc
index ec84b58..e2dc8aa 100644
--- a/aos/events/logging/logfile_utils_test.cc
+++ b/aos/events/logging/logfile_utils_test.cc
@@ -1,6 +1,7 @@
#include "aos/events/logging/logfile_utils.h"
#include <chrono>
+#include <filesystem>
#include <random>
#include <string>
@@ -29,13 +30,13 @@
// Adapter class to make it easy to test DetachedBufferWriter without adding
// test only boilerplate to DetachedBufferWriter.
-class TestDetachedBufferWriter : public DetachedBufferWriter {
+class TestDetachedBufferWriter : public DetachedBufferFileWriter {
public:
// Pick a max size that is rather conservative.
static constexpr size_t kMaxMessageSize = 128 * 1024;
TestDetachedBufferWriter(std::string_view filename)
- : DetachedBufferWriter(filename,
- std::make_unique<DummyEncoder>(kMaxMessageSize)) {}
+ : DetachedBufferFileWriter(
+ filename, std::make_unique<DummyEncoder>(kMaxMessageSize)) {}
void WriteSizedFlatbuffer(flatbuffers::DetachedBuffer &&buffer) {
QueueSpan(absl::Span<const uint8_t>(buffer.data(), buffer.size()));
}
@@ -154,6 +155,7 @@
writer.QueueSpan(m2.span());
writer.QueueSpan(m3.span());
}
+ ASSERT_TRUE(std::filesystem::exists(logfile0)) << logfile0;
const std::vector<LogFile> parts = SortParts({logfile0});
diff --git a/aos/events/logging/logger_main.cc b/aos/events/logging/logger_main.cc
index f0582dc..0dbfc8d 100644
--- a/aos/events/logging/logger_main.cc
+++ b/aos/events/logging/logger_main.cc
@@ -48,8 +48,7 @@
aos::ShmEventLoop event_loop(&config.message());
- std::unique_ptr<aos::logger::MultiNodeLogNamer> log_namer;
- log_namer = std::make_unique<aos::logger::MultiNodeLogNamer>(
+ auto log_namer = std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
absl::StrCat(aos::logging::GetLogName("fbs_log"), "/"), &event_loop);
if (FLAGS_snappy_compress) {
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 80b3a4a..fcbd77b 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -1,5 +1,7 @@
#include <sys/stat.h>
+#include <filesystem>
+
#include "absl/strings/str_format.h"
#include "aos/events/event_loop.h"
#include "aos/events/logging/log_reader.h"
@@ -88,6 +90,8 @@
event_loop_factory_.RunFor(chrono::milliseconds(20000));
}
+ ASSERT_TRUE(std::filesystem::exists(logfile));
+
// Even though it doesn't make any difference here, exercise the logic for
// passing in a separate config.
LogReader reader(logfile, &config_.message());
@@ -152,16 +156,16 @@
Logger logger(logger_event_loop.get());
logger.set_polling_period(std::chrono::milliseconds(100));
- logger_event_loop->OnRun(
- [base_name1, base_name2, &logger_event_loop, &logger]() {
- logger.StartLogging(std::make_unique<MultiNodeLogNamer>(
- base_name1, logger_event_loop->configuration(),
- logger_event_loop.get(), logger_event_loop->node()));
- EXPECT_DEATH(logger.StartLogging(std::make_unique<MultiNodeLogNamer>(
- base_name2, logger_event_loop->configuration(),
- logger_event_loop.get(), logger_event_loop->node())),
- "Already logging");
- });
+ logger_event_loop->OnRun([base_name1, base_name2, &logger_event_loop,
+ &logger]() {
+ logger.StartLogging(std::make_unique<MultiNodeFilesLogNamer>(
+ base_name1, logger_event_loop->configuration(),
+ logger_event_loop.get(), logger_event_loop->node()));
+ EXPECT_DEATH(logger.StartLogging(std::make_unique<MultiNodeFilesLogNamer>(
+ base_name2, logger_event_loop->configuration(),
+ logger_event_loop.get(), logger_event_loop->node())),
+ "Already logging");
+ });
event_loop_factory_.RunFor(chrono::milliseconds(20000));
}
}
@@ -229,7 +233,7 @@
logger.set_separate_config(false);
logger.set_polling_period(std::chrono::milliseconds(100));
logger_event_loop->OnRun([base_name, &logger_event_loop, &logger]() {
- logger.StartLogging(std::make_unique<MultiNodeLogNamer>(
+ logger.StartLogging(std::make_unique<MultiNodeFilesLogNamer>(
base_name, logger_event_loop->configuration(),
logger_event_loop.get(), logger_event_loop->node()));
logger.StopLogging(aos::monotonic_clock::min_time);
@@ -267,13 +271,13 @@
Logger logger(logger_event_loop.get());
logger.set_separate_config(false);
logger.set_polling_period(std::chrono::milliseconds(100));
- logger.StartLogging(std::make_unique<MultiNodeLogNamer>(
+ logger.StartLogging(std::make_unique<MultiNodeFilesLogNamer>(
base_name1, logger_event_loop->configuration(), logger_event_loop.get(),
logger_event_loop->node()));
event_loop_factory_.RunFor(chrono::milliseconds(10000));
logger.StopLogging(logger_event_loop->monotonic_now());
event_loop_factory_.RunFor(chrono::milliseconds(10000));
- logger.StartLogging(std::make_unique<MultiNodeLogNamer>(
+ logger.StartLogging(std::make_unique<MultiNodeFilesLogNamer>(
base_name2, logger_event_loop->configuration(), logger_event_loop.get(),
logger_event_loop->node()));
event_loop_factory_.RunFor(chrono::milliseconds(10000));
diff --git a/aos/events/logging/multinode_logger_test_lib.cc b/aos/events/logging/multinode_logger_test_lib.cc
index 0f588c6..3ffb091 100644
--- a/aos/events/logging/multinode_logger_test_lib.cc
+++ b/aos/events/logging/multinode_logger_test_lib.cc
@@ -41,8 +41,8 @@
logger->set_logger_version(
absl::StrCat("logger_version_", event_loop->node()->name()->str()));
event_loop->OnRun([this, logfile_base]() {
- std::unique_ptr<MultiNodeLogNamer> namer =
- std::make_unique<MultiNodeLogNamer>(logfile_base, configuration,
+ std::unique_ptr<MultiNodeFilesLogNamer> namer =
+ std::make_unique<MultiNodeFilesLogNamer>(logfile_base, configuration,
event_loop.get(), node);
namer->set_extension(params.extension);
namer->set_encoder_factory(params.encoder_factory);
diff --git a/aos/events/logging/realtime_replay_test.cc b/aos/events/logging/realtime_replay_test.cc
index 2a5703a..2d02766 100644
--- a/aos/events/logging/realtime_replay_test.cc
+++ b/aos/events/logging/realtime_replay_test.cc
@@ -191,8 +191,8 @@
logger.set_separate_config(false);
logger.set_polling_period(std::chrono::milliseconds(100));
- std::unique_ptr<MultiNodeLogNamer> namer =
- std::make_unique<MultiNodeLogNamer>(
+ std::unique_ptr<MultiNodeFilesLogNamer> namer =
+ std::make_unique<MultiNodeFilesLogNamer>(
base_name_, &config_.message(), logger_event_loop.get(),
configuration::GetNode(&config_.message(), "pi1"));
@@ -236,8 +236,8 @@
logger.set_separate_config(false);
logger.set_polling_period(std::chrono::milliseconds(100));
- std::unique_ptr<MultiNodeLogNamer> namer =
- std::make_unique<MultiNodeLogNamer>(
+ std::unique_ptr<MultiNodeFilesLogNamer> namer =
+ std::make_unique<MultiNodeFilesLogNamer>(
base_name_, &config_.message(), logger_event_loop.get(),
configuration::GetNode(&config_.message(), "pi1"));
@@ -287,8 +287,8 @@
Logger logger(logger_event_loop.get());
logger.set_separate_config(false);
logger.set_polling_period(std::chrono::milliseconds(100));
- std::unique_ptr<MultiNodeLogNamer> namer =
- std::make_unique<MultiNodeLogNamer>(
+ std::unique_ptr<MultiNodeFilesLogNamer> namer =
+ std::make_unique<MultiNodeFilesLogNamer>(
base_name_, &config_.message(), logger_event_loop.get(),
configuration::GetNode(&config_.message(), "pi1"));
@@ -348,8 +348,8 @@
logger.set_separate_config(false);
logger.set_polling_period(std::chrono::milliseconds(100));
- std::unique_ptr<MultiNodeLogNamer> namer =
- std::make_unique<MultiNodeLogNamer>(
+ std::unique_ptr<MultiNodeFilesLogNamer> namer =
+ std::make_unique<MultiNodeFilesLogNamer>(
base_name_, &config_.message(), logger_event_loop.get(),
configuration::GetNode(&config_.message(), "pi1"));
diff --git a/frc971/input/BUILD b/frc971/input/BUILD
index ddb1f43..8515153 100644
--- a/frc971/input/BUILD
+++ b/frc971/input/BUILD
@@ -58,12 +58,28 @@
)
cc_library(
+ name = "redundant_joystick_data",
+ srcs = [
+ "redundant_joystick_data.cc",
+ ],
+ hdrs = [
+ "redundant_joystick_data.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":driver_station_data",
+ "//aos/events:event_loop",
+ ],
+)
+
+cc_library(
name = "action_joystick_input",
srcs = ["action_joystick_input.cc"],
hdrs = ["action_joystick_input.h"],
target_compatible_with = ["@platforms//os:linux"],
deps = [
":drivetrain_input",
+ ":redundant_joystick_data",
"//aos:init",
"//aos/actions:action_lib",
"//aos/logging",
diff --git a/frc971/input/action_joystick_input.cc b/frc971/input/action_joystick_input.cc
index 4f1d655..dfd229e 100644
--- a/frc971/input/action_joystick_input.cc
+++ b/frc971/input/action_joystick_input.cc
@@ -4,6 +4,7 @@
#include "frc971/autonomous/auto_mode_generated.h"
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/input/driver_station_data.h"
+#include "frc971/input/redundant_joystick_data.h"
using ::frc971::input::driver_station::ControlBit;
@@ -11,6 +12,16 @@
namespace input {
void ActionJoystickInput::RunIteration(
+ const ::frc971::input::driver_station::Data &unsorted_data) {
+ if (input_config_.use_redundant_joysticks) {
+ driver_station::RedundantData redundant_data_storage(unsorted_data);
+ DoRunIteration(redundant_data_storage);
+ } else {
+ DoRunIteration(unsorted_data);
+ }
+}
+
+void ActionJoystickInput::DoRunIteration(
const ::frc971::input::driver_station::Data &data) {
const bool last_auto_running = auto_running_;
auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
diff --git a/frc971/input/action_joystick_input.h b/frc971/input/action_joystick_input.h
index 3284b15..f77f416 100644
--- a/frc971/input/action_joystick_input.h
+++ b/frc971/input/action_joystick_input.h
@@ -25,6 +25,11 @@
// A button, for use with the run_teleop_in_auto, that will cancel the auto
// mode, and if run_telop_in_auto is specified, resume teloperation.
const driver_station::ButtonLocation cancel_auto_button = {-1, -1};
+
+ // Use button 14 and 15 to encode the id of the joystick and remap the
+ // joysticks so that their ids are independent of their order on the
+ // driverstation.
+ bool use_redundant_joysticks = false;
};
ActionJoystickInput(
::aos::EventLoop *event_loop,
@@ -95,6 +100,8 @@
void RunIteration(const ::frc971::input::driver_station::Data &data) override;
+ void DoRunIteration(const ::frc971::input::driver_station::Data &data);
+
void StartAuto();
void StopAuto();
diff --git a/frc971/input/driver_station_data.h b/frc971/input/driver_station_data.h
index c5c38b4..5ebf8f4 100644
--- a/frc971/input/driver_station_data.h
+++ b/frc971/input/driver_station_data.h
@@ -76,24 +76,24 @@
// Updates the current information with a new set of values.
void Update(const aos::JoystickState *new_values);
- bool IsPressed(POVLocation location) const;
- bool PosEdge(POVLocation location) const;
- bool NegEdge(POVLocation location) const;
+ virtual bool IsPressed(POVLocation location) const;
+ virtual bool PosEdge(POVLocation location) const;
+ virtual bool NegEdge(POVLocation location) const;
// Returns the current and previous "values" for the POV.
- int32_t GetPOV(int joystick) const;
- int32_t GetOldPOV(int joystick) const;
+ virtual int32_t GetPOV(int joystick) const;
+ virtual int32_t GetOldPOV(int joystick) const;
- bool IsPressed(ButtonLocation location) const;
- bool PosEdge(ButtonLocation location) const;
- bool NegEdge(ButtonLocation location) const;
+ virtual bool IsPressed(ButtonLocation location) const;
+ virtual bool PosEdge(ButtonLocation location) const;
+ virtual bool NegEdge(ButtonLocation location) const;
- bool GetControlBit(ControlBit bit) const;
- bool PosEdge(ControlBit bit) const;
- bool NegEdge(ControlBit bit) const;
+ virtual bool GetControlBit(ControlBit bit) const;
+ virtual bool PosEdge(ControlBit bit) const;
+ virtual bool NegEdge(ControlBit bit) const;
// Returns the value in the range [-1.0, 1.0].
- float GetAxis(JoystickAxis axis) const;
+ virtual float GetAxis(JoystickAxis axis) const;
private:
struct SavedJoystickState {
diff --git a/frc971/input/drivetrain_input.cc b/frc971/input/drivetrain_input.cc
index 239eddf..5aedd0e 100644
--- a/frc971/input/drivetrain_input.cc
+++ b/frc971/input/drivetrain_input.cc
@@ -280,7 +280,7 @@
const ButtonLocation kSecondButton(1, 2);
const ButtonLocation kBottomButton(1, 4);
// Non-existant button for nops.
- const ButtonLocation kDummyButton(1, 10);
+ const ButtonLocation kDummyButton(1, 15);
// TODO(james): Make a copy assignment operator for ButtonLocation so we don't
// have to shoehorn in these ternary operators.
diff --git a/frc971/input/redundant_joystick_data.cc b/frc971/input/redundant_joystick_data.cc
new file mode 100644
index 0000000..3274856
--- /dev/null
+++ b/frc971/input/redundant_joystick_data.cc
@@ -0,0 +1,100 @@
+#include "frc971/input/redundant_joystick_data.h"
+
+#include "aos/logging/logging.h"
+
+namespace frc971 {
+namespace input {
+namespace driver_station {
+
+RedundantData::RedundantData(const Data &data) : joystick_map_(), data_(data) {
+ // Start with a naive map.
+ for (int i = 0; i < JoystickFeature::kJoysticks; i++) {
+ joystick_map_.at(i) = i;
+ }
+
+ for (int i = 0; i < JoystickFeature::kJoysticks; i++) {
+ ButtonLocation id_bit0_location(i + 1, kIdBit0Button);
+ ButtonLocation id_bit1_location(i + 1, kIdBit1Button);
+ ButtonLocation redundant_bit_location(i + 1, kRedundantBitButton);
+
+ int id_bit0 = data_.IsPressed(id_bit0_location);
+ int id_bit1 = data_.IsPressed(id_bit1_location);
+ int is_redundant = data_.IsPressed(redundant_bit_location);
+
+ // We don't care if this is the redundant or primary one. Pick the second
+ // one.
+ (void)is_redundant;
+
+ int packed_joystick_number = (id_bit1 << 1u) | (id_bit0 << 0u);
+
+ joystick_map_.at(packed_joystick_number) = i + 1;
+ }
+};
+
+int RedundantData::MapRedundantJoystick(int joystick) const {
+ return joystick_map_.at(joystick);
+}
+
+bool RedundantData::IsPressed(POVLocation location) const {
+ POVLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.IsPressed(mapped_location);
+}
+
+bool RedundantData::PosEdge(POVLocation location) const {
+ POVLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.PosEdge(mapped_location);
+}
+
+bool RedundantData::NegEdge(POVLocation location) const {
+ POVLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.NegEdge(mapped_location);
+}
+
+// Returns the current and previous "values" for the POV.
+int32_t RedundantData::GetPOV(int joystick) const {
+ return data_.GetPOV(MapRedundantJoystick(joystick));
+}
+
+int32_t RedundantData::GetOldPOV(int joystick) const {
+ return data_.GetOldPOV(MapRedundantJoystick(joystick));
+}
+
+bool RedundantData::IsPressed(ButtonLocation location) const {
+ ButtonLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.IsPressed(mapped_location);
+}
+
+bool RedundantData::PosEdge(ButtonLocation location) const {
+ ButtonLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.PosEdge(mapped_location);
+}
+
+bool RedundantData::NegEdge(ButtonLocation location) const {
+ ButtonLocation mapped_location(MapRedundantJoystick(location.joystick()),
+ location.number());
+ return data_.NegEdge(mapped_location);
+}
+
+bool RedundantData::GetControlBit(ControlBit bit) const {
+ return data_.GetControlBit(bit);
+}
+
+bool RedundantData::PosEdge(ControlBit bit) const { return data_.PosEdge(bit); }
+
+bool RedundantData::NegEdge(ControlBit bit) const { return data_.NegEdge(bit); }
+
+// Returns the value in the range [-1.0, 1.0].
+float RedundantData::GetAxis(JoystickAxis axis) const {
+ JoystickAxis mapped_location(MapRedundantJoystick(axis.joystick()),
+ axis.number());
+ return data_.GetAxis(mapped_location);
+}
+
+} // namespace driver_station
+} // namespace input
+} // namespace frc971
diff --git a/frc971/input/redundant_joystick_data.h b/frc971/input/redundant_joystick_data.h
new file mode 100644
index 0000000..c6cdb78
--- /dev/null
+++ b/frc971/input/redundant_joystick_data.h
@@ -0,0 +1,58 @@
+#ifndef AOS_INPUT_REDUNDANT_JOYSTICK_DATA_H_
+#define AOS_INPUT_REDUNDANT_JOYSTICK_DATA_H_
+
+#include "frc971/input/driver_station_data.h"
+
+namespace frc971 {
+namespace input {
+namespace driver_station {
+
+// A class to wrap driver_station::Data and map logical joystick numbers to
+// their actual numbers in the order they are on the driverstation.
+//
+// Bits 13 and 14 of the joystick bitmap are defined to be a two bit number
+// corresponding to the joystick's logical joystick number.
+class RedundantData : public Data {
+ public:
+ RedundantData(const Data &data);
+
+ bool IsPressed(POVLocation location) const override;
+ bool PosEdge(POVLocation location) const override;
+ bool NegEdge(POVLocation location) const override;
+
+ // Returns the current and previous "values" for the POV.
+ int32_t GetPOV(int joystick) const override;
+ int32_t GetOldPOV(int joystick) const override;
+
+ bool IsPressed(ButtonLocation location) const override;
+ bool PosEdge(ButtonLocation location) const override;
+ bool NegEdge(ButtonLocation location) const override;
+
+ bool GetControlBit(ControlBit bit) const override;
+ bool PosEdge(ControlBit bit) const override;
+ bool NegEdge(ControlBit bit) const override;
+
+ // Returns the value in the range [-1.0, 1.0].
+ float GetAxis(JoystickAxis axis) const override;
+
+ private:
+ static constexpr int kIdBit0Button = 14;
+ static constexpr int kIdBit1Button = 15;
+ static constexpr int kRedundantBitButton = 16;
+
+ int MapRedundantJoystick(int joystick) const;
+
+ // A mapping from logical joystick numbers to their actual order on the
+ // driverstation.
+ //
+ // Index is logical joystick number, Value is mapped joystick number.
+ std::array<int, JoystickFeature::kJoysticks> joystick_map_;
+
+ const Data &data_;
+};
+
+} // namespace driver_station
+} // namespace input
+} // namespace frc971
+
+#endif // AOS_INPUT_REDUNDANT_JOYSTICK_DATA_H_
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index dea883b..ddeba10 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -235,9 +235,9 @@
}
std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById(
- TargetId target_id) {
+ TargetId target_id) const {
if (target_poses_.count(target_id) > 0) {
- return TargetMapper::TargetPose{target_id, target_poses_[target_id]};
+ return TargetMapper::TargetPose{target_id, target_poses_.at(target_id)};
}
return std::nullopt;
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 35c0977..f5d1aef 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -47,7 +47,7 @@
std::vector<TargetPose> target_poses, TargetId target_id);
// Version that gets based on internal target_poses
- std::optional<TargetPose> GetTargetPoseById(TargetId target_id);
+ std::optional<TargetPose> GetTargetPoseById(TargetId target_id) const;
ceres::examples::MapOfPoses target_poses() { return target_poses_; }
diff --git a/frc971/vision/visualize_robot.h b/frc971/vision/visualize_robot.h
index cd8b4d0..31d584d 100644
--- a/frc971/vision/visualize_robot.h
+++ b/frc971/vision/visualize_robot.h
@@ -21,13 +21,16 @@
class VisualizeRobot {
public:
+ VisualizeRobot(cv::Size default_size = cv::Size(1280, 720))
+ : default_size_(default_size) {}
+
// Set image on which to draw
void SetImage(cv::Mat image) { image_ = image; }
// Sets image to all black.
- // Uses default_size if no image has been set yet, else image_.size()
- void ClearImage(cv::Size default_size = cv::Size(1280, 720)) {
- auto image_size = (image_.data == nullptr ? default_size : image_.size());
+ // Uses default_size_ if no image has been set yet, else image_.size()
+ void ClearImage() {
+ auto image_size = (image_.data == nullptr ? default_size_ : image_.size());
cv::Mat black_image_mat = cv::Mat::zeros(image_size, CV_8UC3);
SetImage(black_image_mat);
}
@@ -73,9 +76,10 @@
Eigen::Affine3d H_world_viewpoint_; // Where to view the world from
cv::Mat image_; // Image to draw on
- cv::Mat camera_mat_; // Virtual camera intrinsics (defines fov, center)
- cv::Mat dist_coeffs_; // Distortion coefficients, if desired (only used in
- // DrawFrameAxes
+ cv::Mat camera_mat_; // Virtual camera intrinsics (defines fov, center)
+ cv::Mat dist_coeffs_; // Distortion coefficients, if desired (only used in
+ // DrawFrameAxes
+ cv::Size default_size_; // Default image size
};
} // namespace vision
} // namespace frc971
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index d2f2880..57feabb 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -31,7 +31,7 @@
LoggerState(aos::logger::LogReader *reader, const aos::Node *node)
: event_loop_(
reader->event_loop_factory()->MakeEventLoop("logger", node)),
- namer_(std::make_unique<aos::logger::MultiNodeLogNamer>(
+ namer_(std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
absl::StrCat(FLAGS_output_folder, "/", node->name()->string_view(),
"/"),
event_loop_.get())),
diff --git a/y2022/localizer/localizer_replay.cc b/y2022/localizer/localizer_replay.cc
index 08479eb..0c09535 100644
--- a/y2022/localizer/localizer_replay.cc
+++ b/y2022/localizer/localizer_replay.cc
@@ -21,7 +21,7 @@
LoggerState(aos::logger::LogReader *reader, const aos::Node *node)
: event_loop_(
reader->event_loop_factory()->MakeEventLoop("logger", node)),
- namer_(std::make_unique<aos::logger::MultiNodeLogNamer>(
+ namer_(std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
absl::StrCat(FLAGS_output_folder, "/", node->name()->string_view(),
"/"),
event_loop_.get())),
diff --git a/y2023/BUILD b/y2023/BUILD
index 08713b7..11600de 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -322,6 +322,7 @@
"//frc971/input:action_joystick_input",
"//frc971/input:drivetrain_input",
"//frc971/input:joystick_input",
+ "//frc971/input:redundant_joystick_data",
"//y2023/control_loops/drivetrain:drivetrain_base",
"//y2023/control_loops/drivetrain:target_selector_hint_fbs",
"//y2023/control_loops/superstructure:superstructure_goal_fbs",
diff --git a/y2023/constants.cc b/y2023/constants.cc
index 07b2ee6..2ab296f 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -83,24 +83,24 @@
break;
case kCompTeamNumber:
- arm_proximal->zeroing.measured_absolute_position = 0.153241637089465;
+ arm_proximal->zeroing.measured_absolute_position = 0.152273750996612;
arm_proximal->potentiometer_offset =
0.931355973012855 + 8.6743197253382 - 0.101200335326309 -
0.0820901660993467 - 0.0703733798337964 - 0.0294645384848748 -
- 0.577156175549626;
+ 0.577156175549626 - 0.000944609125286267;
- arm_distal->zeroing.measured_absolute_position = 0.137511584277487;
+ arm_distal->zeroing.measured_absolute_position = 0.201204843551682;
arm_distal->potentiometer_offset =
0.436664933370656 + 0.49457213779426 + 6.78213223139724 -
0.0220711555235029 - 0.0162945074111813 + 0.00630344935527365 -
0.0164398318919943 - 0.145833494945215 + 0.234878799868491 +
0.125924230298394 + 0.147136306208754 - 0.69167546169753 -
- 0.308761538844425;
+ 0.308761538844425 + 0.610386472488493 + 0.08384162885249 + 0.0262274735196811;
arm_distal->zeroing.one_revolution_distance =
M_PI * 2.0 * constants::Values::kDistalEncoderRatio();
- roll_joint->zeroing.measured_absolute_position = 0.619108755444215;
+ roll_joint->zeroing.measured_absolute_position = 0.419144048980465;
roll_joint->potentiometer_offset =
-(3.87038557084874 - 0.0241774522172967 + 0.0711345168020632 -
0.866186131631967 - 0.0256788357596952 + 0.18101759154572017 -
@@ -108,37 +108,39 @@
0.5935210745062 + 0.166256655718334 - 0.12591438680483 +
0.11972765117321 - 0.318724743041507) +
0.0201047336425017 - 1.0173426655158 - 0.186085272847293 -
- 0.0317706563397807;
+ 0.0317706563397807 - 2.6357823523782 + 0.871932806570122;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 2.29414865465015;
+ 2.29036834725319;
break;
case kPracticeTeamNumber:
- arm_proximal->zeroing.measured_absolute_position = 0.24572544970387;
+ arm_proximal->zeroing.measured_absolute_position = 0.140348044909775;
arm_proximal->potentiometer_offset =
10.5178592988554 + 0.0944609125285876 - 0.00826532984625095 +
- 0.167359305216504;
+ 0.167359305216504 + 0.135144500925909;
- arm_distal->zeroing.measured_absolute_position = 0.0698375027814462;
+ arm_distal->zeroing.measured_absolute_position = 0.860219239796068;
arm_distal->potentiometer_offset =
7.673132586937 - 0.0799284644472573 - 0.0323574039310657 +
0.0143810684138064 + 0.00945555248207735 + 0.452446388633863 +
- 0.0194863477007102;
+ 0.0194863477007102 + 0.235993332670562 + 0.00138417783482921;
arm_distal->zeroing.one_revolution_distance =
M_PI * 2.0 * constants::Values::kDistalEncoderRatio() *
- 3.11964893168338 / 3.148;
+ //3.11964893168338 / 3.148;
+ (3.12725165289659 + 0.002) / 3.1485739705977704;
- roll_joint->zeroing.measured_absolute_position = 0.0722321007692069;
+ roll_joint->zeroing.measured_absolute_position = 1.78072413296938;
roll_joint->potentiometer_offset =
0.624713611895747 + 3.10458504917251 - 0.0966407797407789 +
0.0257708772364788 - 0.0395076737853459 - 6.87914956118006 -
- 0.097581301615046 + 3.3424421683095 - 3.97605190912604;
+ 0.097581301615046 + 3.3424421683095 - 3.97605190912604 +
+ 0.709274294168941;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 1.04410369068834;
+ 6.05398282057911;
break;
diff --git a/y2023/constants.h b/y2023/constants.h
index f404472..2f16f1a 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -121,9 +121,7 @@
// Wrist
static constexpr double kWristEncoderCountsPerRevolution() { return 4096.0; }
- static constexpr double kCompWristEncoderRatio() {
- return 1.0;
- }
+ static constexpr double kCompWristEncoderRatio() { return 1.0; }
static constexpr double kPracticeWristEncoderRatio() {
return (24.0 / 36.0) * (36.0 / 60.0);
}
@@ -172,7 +170,7 @@
}
// if true, tune down all the arm constants for testing.
- static constexpr bool kArmGrannyMode() { return false; }
+ static constexpr bool kArmGrannyMode() { return true; }
// the operating voltage.
static constexpr double kArmOperatingVoltage() {
diff --git a/y2023/control_loops/superstructure/superstructure_main.cc b/y2023/control_loops/superstructure/superstructure_main.cc
index 10a9ca9..4a044b6 100644
--- a/y2023/control_loops/superstructure/superstructure_main.cc
+++ b/y2023/control_loops/superstructure/superstructure_main.cc
@@ -16,6 +16,8 @@
::aos::ShmEventLoop event_loop(&config.message());
+ frc971::constants::WaitForConstants<y2023::Constants>(&config.message());
+
auto trajectories =
y2023::control_loops::superstructure::Superstructure::GetArmTrajectories(
FLAGS_arm_trajectories);
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index efd7537..a8f540f 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -16,6 +16,7 @@
#include "frc971/input/driver_station_data.h"
#include "frc971/input/drivetrain_input.h"
#include "frc971/input/joystick_input.h"
+#include "frc971/input/redundant_joystick_data.h"
#include "frc971/zeroing/wrap.h"
#include "y2023/constants.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
@@ -46,35 +47,34 @@
constexpr double kCubeWrist = 1.0;
// TODO(milind): add correct locations
-const ButtonLocation kDriverSpit(2, 1);
-const ButtonLocation kSpit(4, 13);
+const ButtonLocation kDriverSpit(1, 1);
+const ButtonLocation kSpit(2, 5);
-const ButtonLocation kHighConeScoreLeft(4, 14);
-const ButtonLocation kHighConeScoreRight(3, 1);
+const ButtonLocation kHighConeScoreLeft(2, 6);
+const ButtonLocation kHighConeScoreRight(2, 9);
-const ButtonLocation kMidConeScoreLeft(4, 15);
-const ButtonLocation kMidConeScoreRight(3, 2);
+const ButtonLocation kMidConeScoreLeft(2, 7);
+const ButtonLocation kMidConeScoreRight(2, 10);
-const ButtonLocation kLowConeScoreLeft(4, 16);
-const ButtonLocation kLowConeScoreRight(3, 3);
+const ButtonLocation kLowConeScoreLeft(2, 8);
+const ButtonLocation kLowConeScoreRight(2, 11);
-const ButtonLocation kHighCube(4, 1);
-const ButtonLocation kMidCube(4, 2);
-const ButtonLocation kLowCube(4, 3);
+const ButtonLocation kHighCube(1, 6);
+const ButtonLocation kMidCube(1, 7);
+const ButtonLocation kLowCube(1, 8);
-const ButtonLocation kGroundPickupConeUp(4, 7);
-const ButtonLocation kGroundPickupConeDown(4, 8);
-const ButtonLocation kGroundPickupCube(4, 10);
-const ButtonLocation kHPConePickup(4, 6);
+const ButtonLocation kGroundPickupConeUp(1, 12);
+const ButtonLocation kGroundPickupConeDown(1, 13);
+const ButtonLocation kGroundPickupCube(2, 2);
+const ButtonLocation kHPConePickup(1, 11);
-const ButtonLocation kSuck(4, 11);
-const ButtonLocation kBack(4, 12);
+const ButtonLocation kSuck(2, 3);
+const ButtonLocation kBack(2, 4);
-const ButtonLocation kWrist(4, 10);
-const ButtonLocation kStayIn(3, 4);
+const ButtonLocation kStayIn(2, 12);
-const ButtonLocation kConeDownTip(4, 4);
-const ButtonLocation kConeDownBase(4, 5);
+const ButtonLocation kConeDownTip(1, 9);
+const ButtonLocation kConeDownBase(1, 10);
namespace superstructure = y2023::control_loops::superstructure;
namespace arm = superstructure::arm;
@@ -369,7 +369,8 @@
: ::frc971::input::ActionJoystickInput(
event_loop,
::y2023::control_loops::drivetrain::GetDrivetrainConfig(),
- ::frc971::input::DrivetrainInputReader::InputType::kPistol, {}),
+ ::frc971::input::DrivetrainInputReader::InputType::kPistol,
+ {.use_redundant_joysticks = true}),
superstructure_goal_sender_(
event_loop->MakeSender<superstructure::Goal>("/superstructure")),
target_selector_hint_sender_(
diff --git a/y2023/localizer/localizer_replay.cc b/y2023/localizer/localizer_replay.cc
index 7176f4e..c74b708 100644
--- a/y2023/localizer/localizer_replay.cc
+++ b/y2023/localizer/localizer_replay.cc
@@ -20,7 +20,7 @@
LoggerState(aos::logger::LogReader *reader, const aos::Node *node)
: event_loop_(
reader->event_loop_factory()->MakeEventLoop("logger", node)),
- namer_(std::make_unique<aos::logger::MultiNodeLogNamer>(
+ namer_(std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
absl::StrCat(FLAGS_output_folder, "/", node->name()->string_view(),
"/"),
event_loop_.get())),
diff --git a/y2023/vision/image_logger.cc b/y2023/vision/image_logger.cc
index d90f9c2..e8b6bb2 100644
--- a/y2023/vision/image_logger.cc
+++ b/y2023/vision/image_logger.cc
@@ -18,9 +18,9 @@
DEFINE_double(disabled_time, 5.0,
"Continue logging if disabled for this amount of time or less");
-std::unique_ptr<aos::logger::MultiNodeLogNamer> MakeLogNamer(
+std::unique_ptr<aos::logger::MultiNodeFilesLogNamer> MakeLogNamer(
aos::EventLoop *event_loop) {
- return std::make_unique<aos::logger::MultiNodeLogNamer>(
+ return std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
absl::StrCat(aos::logging::GetLogName("fbs_log"), "/"), event_loop);
}
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 4da972a..3f5355a 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -41,6 +41,9 @@
DEFINE_uint64(wait_key, 0,
"Time in ms to wait between images, if no click (0 to wait "
"indefinitely until click).");
+DEFINE_double(pause_on_distance, 1.0,
+ "Pause if two consecutive implied robot positions differ by more "
+ "than this many meters");
DEFINE_uint64(skip_to, 1,
"Start at combined image of this number (1 is the first image)");
DEFINE_bool(solve, true, "Whether to solve for the field's target map.");
@@ -55,34 +58,165 @@
using frc971::vision::VisualizeRobot;
namespace calibration = frc971::vision::calibration;
-// Change reference frame from camera to robot
-Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camera_target,
- Eigen::Affine3d extrinsics) {
- const Eigen::Affine3d H_robot_camera = extrinsics;
- const Eigen::Affine3d H_robot_target = H_robot_camera * H_camera_target;
- return H_robot_target;
-}
+// Class to handle reading target poses from a replayed log,
+// displaying various debug info, and passing the poses to
+// frc971::vision::TargetMapper for field mapping.
+class TargetMapperReplay {
+ public:
+ TargetMapperReplay(aos::logger::LogReader *reader);
+ ~TargetMapperReplay();
-const int kImageWidth = 1000;
-// Map from pi node name to color for drawing
-const std::map<std::string, cv::Scalar> kPiColors = {
+ // Solves for the target poses with the accumulated detections if FLAGS_solve.
+ void MaybeSolve();
+
+ private:
+ static constexpr int kImageWidth = 1280;
+ // Map from pi node name to color for drawing
+ static const std::map<std::string, cv::Scalar> kPiColors;
+ // Contains fixed target poses without solving, for use with visualization
+ static const TargetMapper kFixedTargetMapper;
+
+ // Change reference frame from camera to robot
+ static Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camera_target,
+ Eigen::Affine3d extrinsics);
+
+ // Adds april tag detections into the detection list, and handles
+ // visualization
+ void HandleAprilTags(const TargetMap &map,
+ aos::distributed_clock::time_point pi_distributed_time,
+ std::string node_name, Eigen::Affine3d extrinsics);
+
+ // Gets images from the given pi and passes apriltag positions to
+ // HandleAprilTags()
+ void HandlePiCaptures(aos::EventLoop *detection_event_loop,
+ aos::EventLoop *mapping_event_loop);
+
+ aos::logger::LogReader *reader_;
+ // April tag detections from all pis
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections_;
+ std::vector<std::unique_ptr<AprilRoboticsDetector>> detectors_;
+
+ VisualizeRobot vis_robot_;
+ // Set of node names which are currently drawn on the display
+ std::set<std::string> drawn_nodes_;
+ // Number of frames displayed
+ size_t display_count_;
+ // Last time we drew onto the display image.
+ // This is different from when we actually call imshow() to update
+ // the display window
+ aos::distributed_clock::time_point last_draw_time_;
+
+ Eigen::Affine3d last_H_world_robot_;
+ // Maximum distance between consecutive T_world_robot's in one display frame,
+ // used to determine if we need to pause for the user to see this frame
+ // clearly
+ double max_delta_T_world_robot_;
+
+ std::vector<std::unique_ptr<aos::EventLoop>> detection_event_loops_;
+ std::vector<std::unique_ptr<aos::EventLoop>> mapping_event_loops_;
+
+ std::unique_ptr<aos::EventLoop> mcap_event_loop_;
+ std::unique_ptr<aos::McapLogger> relogger_;
+};
+
+const auto TargetMapperReplay::kPiColors = std::map<std::string, cv::Scalar>{
{"pi1", cv::Scalar(255, 0, 255)},
{"pi2", cv::Scalar(255, 255, 0)},
{"pi3", cv::Scalar(0, 255, 255)},
{"pi4", cv::Scalar(255, 165, 0)},
};
+const auto TargetMapperReplay::kFixedTargetMapper =
+ TargetMapper(FLAGS_json_path, ceres::examples::VectorOfConstraints{});
+
+Eigen::Affine3d TargetMapperReplay::CameraToRobotDetection(
+ Eigen::Affine3d H_camera_target, Eigen::Affine3d extrinsics) {
+ const Eigen::Affine3d H_robot_camera = extrinsics;
+ const Eigen::Affine3d H_robot_target = H_robot_camera * H_camera_target;
+ return H_robot_target;
+}
+
+TargetMapperReplay::TargetMapperReplay(aos::logger::LogReader *reader)
+ : reader_(reader),
+ timestamped_target_detections_(),
+ detectors_(),
+ vis_robot_(cv::Size(1280, 1000)),
+ drawn_nodes_(),
+ display_count_(0),
+ last_draw_time_(aos::distributed_clock::min_time),
+ last_H_world_robot_(Eigen::Matrix4d::Identity()),
+ max_delta_T_world_robot_(0.0) {
+ // Send new april tag poses. This allows us to take a log of images, then
+ // play with the april detection code and see those changes take effect in
+ // mapping
+ constexpr size_t kNumPis = 4;
+ for (size_t i = 1; i <= kNumPis; i++) {
+ reader_->RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
+ "frc971.vision.TargetMap");
+ reader_->RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
+ "foxglove.ImageAnnotations");
+ reader_->RemapLoggedChannel(absl::StrFormat("/pi%u/constants", i),
+ "y2023.Constants");
+ }
+
+ reader_->RemapLoggedChannel("/imu/constants", "y2023.Constants");
+
+ reader_->Register();
+
+ SendSimulationConstants(reader_->event_loop_factory(), FLAGS_team_number,
+ FLAGS_constants_path);
+
+ for (size_t i = 1; i < kNumPis; i++) {
+ std::string node_name = "pi" + std::to_string(i);
+ const aos::Node *pi =
+ aos::configuration::GetNode(reader_->configuration(), node_name);
+ detection_event_loops_.emplace_back(
+ reader_->event_loop_factory()->MakeEventLoop(node_name + "_detection",
+ pi));
+ mapping_event_loops_.emplace_back(
+ reader_->event_loop_factory()->MakeEventLoop(node_name + "_mapping",
+ pi));
+ HandlePiCaptures(
+ detection_event_loops_[detection_event_loops_.size() - 1].get(),
+ mapping_event_loops_[mapping_event_loops_.size() - 1].get());
+ }
+
+ if (!FLAGS_mcap_output_path.empty()) {
+ LOG(INFO) << "Writing out mcap file to " << FLAGS_mcap_output_path;
+ const aos::Node *node =
+ aos::configuration::GetNode(reader_->configuration(), FLAGS_pi);
+ reader_->event_loop_factory()->GetNodeEventLoopFactory(node)->OnStartup(
+ [this, node]() {
+ mcap_event_loop_ =
+ reader_->event_loop_factory()->MakeEventLoop("mcap", node);
+ relogger_ = std::make_unique<aos::McapLogger>(
+ mcap_event_loop_.get(), FLAGS_mcap_output_path,
+ aos::McapLogger::Serialization::kFlatbuffer,
+ aos::McapLogger::CanonicalChannelNames::kShortened,
+ aos::McapLogger::Compression::kLz4);
+ });
+ }
+
+ if (FLAGS_visualize) {
+ vis_robot_.ClearImage();
+ const double kFocalLength = 500.0;
+ vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+ }
+}
+
+TargetMapperReplay::~TargetMapperReplay() {
+ // Clean up all the pointers
+ for (auto &detector_ptr : detectors_) {
+ detector_ptr.reset();
+ }
+}
+
// Add detected apriltag poses relative to the robot to
// timestamped_target_detections
-void HandleAprilTags(const TargetMap &map,
- aos::distributed_clock::time_point pi_distributed_time,
- std::vector<DataAdapter::TimestampedDetection>
- *timestamped_target_detections,
- Eigen::Affine3d extrinsics, std::string node_name,
- frc971::vision::TargetMapper target_loc_mapper,
- std::set<std::string> *drawn_nodes,
- VisualizeRobot *vis_robot, size_t *display_count,
- aos::distributed_clock::time_point *last_draw_time) {
+void TargetMapperReplay::HandleAprilTags(
+ const TargetMap &map,
+ aos::distributed_clock::time_point pi_distributed_time,
+ std::string node_name, Eigen::Affine3d extrinsics) {
bool drew = false;
std::stringstream label;
label << node_name << " - ";
@@ -118,7 +252,7 @@
CHECK(map.has_monotonic_timestamp_ns())
<< "Need detection timestamps for mapping";
- timestamped_target_detections->emplace_back(
+ timestamped_target_detections_.emplace_back(
DataAdapter::TimestampedDetection{
.time = pi_distributed_time,
.H_robot_target = H_robot_target,
@@ -129,25 +263,28 @@
if (FLAGS_visualize) {
// If we've already drawn in the current image,
// display it before clearing and adding the new poses
- if (drawn_nodes->count(node_name) != 0) {
- (*display_count)++;
- cv::putText(vis_robot->image_,
- "Poses #" + std::to_string(*display_count),
+ if (drawn_nodes_.count(node_name) != 0) {
+ display_count_++;
+ cv::putText(vis_robot_.image_,
+ "Poses #" + std::to_string(display_count_),
cv::Point(600, 10), cv::FONT_HERSHEY_PLAIN, 1.0,
cv::Scalar(255, 255, 255));
- if (*display_count >= FLAGS_skip_to) {
- cv::imshow("View", vis_robot->image_);
- cv::waitKey(FLAGS_wait_key);
+ if (display_count_ >= FLAGS_skip_to) {
+ cv::imshow("View", vis_robot_.image_);
+ cv::waitKey(max_delta_T_world_robot_ > FLAGS_pause_on_distance
+ ? 0
+ : FLAGS_wait_key);
+ max_delta_T_world_robot_ = 0.0;
} else {
- VLOG(1) << "At poses #" << std::to_string(*display_count);
+ VLOG(1) << "At poses #" << std::to_string(display_count_);
}
- vis_robot->ClearImage();
- drawn_nodes->clear();
+ vis_robot_.ClearImage();
+ drawn_nodes_.clear();
}
Eigen::Affine3d H_world_target = PoseUtils::Pose3dToAffine3d(
- target_loc_mapper.GetTargetPoseById(target_pose_fbs->id())->pose);
+ kFixedTargetMapper.GetTargetPoseById(target_pose_fbs->id())->pose);
Eigen::Affine3d H_world_robot = H_world_target * H_robot_target.inverse();
VLOG(2) << node_name << ", " << target_pose_fbs->id()
<< ", t = " << pi_distributed_time
@@ -160,44 +297,46 @@
<< (target_pose_fbs->pose_error() / FLAGS_max_pose_error)
<< " ratio " << target_pose_fbs->pose_error_ratio() << " ";
- vis_robot->DrawRobotOutline(H_world_robot,
+ vis_robot_.DrawRobotOutline(H_world_robot,
std::to_string(target_pose_fbs->id()),
kPiColors.at(node_name));
- vis_robot->DrawFrameAxes(H_world_target,
+ vis_robot_.DrawFrameAxes(H_world_target,
std::to_string(target_pose_fbs->id()),
kPiColors.at(node_name));
+
+ double delta_T_world_robot =
+ (H_world_robot.translation() - last_H_world_robot_.translation())
+ .norm();
+ max_delta_T_world_robot_ =
+ std::max(delta_T_world_robot, max_delta_T_world_robot_);
+
drew = true;
- *last_draw_time = pi_distributed_time;
+ last_draw_time_ = pi_distributed_time;
+ last_H_world_robot_ = H_world_robot;
}
}
if (drew) {
size_t pi_number =
static_cast<size_t>(node_name[node_name.size() - 1] - '0');
- cv::putText(vis_robot->image_, label.str(),
+ cv::putText(vis_robot_.image_, label.str(),
cv::Point(10, 10 + 20 * pi_number), cv::FONT_HERSHEY_PLAIN, 1.0,
kPiColors.at(node_name));
- drawn_nodes->emplace(node_name);
- } else if (pi_distributed_time - *last_draw_time >
+ drawn_nodes_.emplace(node_name);
+ } else if (pi_distributed_time - last_draw_time_ >
std::chrono::milliseconds(30) &&
- *display_count >= FLAGS_skip_to) {
+ display_count_ >= FLAGS_skip_to) {
// Clear the image if we haven't draw in a while
- vis_robot->ClearImage();
- cv::imshow("View", vis_robot->image_);
+ vis_robot_.ClearImage();
+ cv::imshow("View", vis_robot_.image_);
cv::waitKey(FLAGS_wait_key);
+ max_delta_T_world_robot_ = 0.0;
}
}
-// Get images from pi and pass apriltag positions to HandleAprilTags()
-void HandlePiCaptures(
- aos::EventLoop *detection_event_loop, aos::EventLoop *mapping_event_loop,
- aos::logger::LogReader *reader,
- std::vector<DataAdapter::TimestampedDetection>
- *timestamped_target_detections,
- std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors,
- std::set<std::string> *drawn_nodes, VisualizeRobot *vis_robot,
- size_t *display_count, aos::distributed_clock::time_point *last_draw_time) {
+void TargetMapperReplay::HandlePiCaptures(aos::EventLoop *detection_event_loop,
+ aos::EventLoop *mapping_event_loop) {
static constexpr std::string_view kImageChannel = "/camera";
// For the robots, we need to flip the image since the cameras are rolled by
// 180 degrees
@@ -210,26 +349,30 @@
cv::cv2eigen(extrinsics_cv, extrinsics_matrix);
const auto extrinsics = Eigen::Affine3d(extrinsics_matrix);
- detectors->emplace_back(std::move(detector_ptr));
-
- ceres::examples::VectorOfConstraints target_constraints;
- frc971::vision::TargetMapper target_loc_mapper(FLAGS_json_path,
- target_constraints);
+ detectors_.emplace_back(std::move(detector_ptr));
mapping_event_loop->MakeWatcher("/camera", [=](const TargetMap &map) {
aos::distributed_clock::time_point pi_distributed_time =
- reader->event_loop_factory()
+ reader_->event_loop_factory()
->GetNodeEventLoopFactory(mapping_event_loop->node())
->ToDistributedClock(aos::monotonic_clock::time_point(
aos::monotonic_clock::duration(map.monotonic_timestamp_ns())));
std::string node_name = detection_event_loop->node()->name()->str();
- HandleAprilTags(map, pi_distributed_time, timestamped_target_detections,
- extrinsics, node_name, target_loc_mapper, drawn_nodes,
- vis_robot, display_count, last_draw_time);
+ HandleAprilTags(map, pi_distributed_time, node_name, extrinsics);
});
}
+void TargetMapperReplay::MaybeSolve() {
+ if (FLAGS_solve) {
+ auto target_constraints =
+ DataAdapter::MatchTargetDetections(timestamped_target_detections_);
+
+ TargetMapper mapper(FLAGS_json_path, target_constraints);
+ mapper.Solve(FLAGS_field_name, FLAGS_output_dir);
+ }
+}
+
void MappingMain(int argc, char *argv[]) {
std::vector<std::string> unsorted_logfiles =
aos::logger::FindLogs(argc, argv);
@@ -241,115 +384,14 @@
? std::nullopt
: std::make_optional(aos::configuration::ReadConfig(FLAGS_config)));
- // open logfiles
+ // Open logfiles
aos::logger::LogReader reader(
aos::logger::SortParts(unsorted_logfiles),
config.has_value() ? &config->message() : nullptr);
- // Send new april tag poses. This allows us to take a log of images, then
- // play with the april detection code and see those changes take effect in
- // mapping
- constexpr size_t kNumPis = 4;
- for (size_t i = 1; i <= kNumPis; i++) {
- reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
- "frc971.vision.TargetMap");
- reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
- "foxglove.ImageAnnotations");
- reader.RemapLoggedChannel(absl::StrFormat("/pi%u/constants", i),
- "y2023.Constants");
- }
- reader.RemapLoggedChannel("/imu/constants", "y2023.Constants");
-
- reader.Register();
-
- SendSimulationConstants(reader.event_loop_factory(), FLAGS_team_number,
- FLAGS_constants_path);
-
- std::vector<std::unique_ptr<AprilRoboticsDetector>> detectors;
- VisualizeRobot vis_robot;
- std::set<std::string> drawn_nodes;
- size_t display_count = 0;
- aos::distributed_clock::time_point last_draw_time =
- aos::distributed_clock::min_time;
-
- const aos::Node *pi1 =
- aos::configuration::GetNode(reader.configuration(), "pi1");
- std::unique_ptr<aos::EventLoop> pi1_detection_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi1_detection", pi1);
- std::unique_ptr<aos::EventLoop> pi1_mapping_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi1_mapping", pi1);
- HandlePiCaptures(pi1_detection_event_loop.get(), pi1_mapping_event_loop.get(),
- &reader, ×tamped_target_detections, &detectors,
- &drawn_nodes, &vis_robot, &display_count, &last_draw_time);
-
- const aos::Node *pi2 =
- aos::configuration::GetNode(reader.configuration(), "pi2");
- std::unique_ptr<aos::EventLoop> pi2_detection_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi2_detection", pi2);
- std::unique_ptr<aos::EventLoop> pi2_mapping_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi2_mapping", pi2);
- HandlePiCaptures(pi2_detection_event_loop.get(), pi2_mapping_event_loop.get(),
- &reader, ×tamped_target_detections, &detectors,
- &drawn_nodes, &vis_robot, &display_count, &last_draw_time);
-
- const aos::Node *pi3 =
- aos::configuration::GetNode(reader.configuration(), "pi3");
- std::unique_ptr<aos::EventLoop> pi3_detection_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi3_detection", pi3);
- std::unique_ptr<aos::EventLoop> pi3_mapping_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi3_mapping", pi3);
- HandlePiCaptures(pi3_detection_event_loop.get(), pi3_mapping_event_loop.get(),
- &reader, ×tamped_target_detections, &detectors,
- &drawn_nodes, &vis_robot, &display_count, &last_draw_time);
-
- const aos::Node *pi4 =
- aos::configuration::GetNode(reader.configuration(), "pi4");
- std::unique_ptr<aos::EventLoop> pi4_detection_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi4_detection", pi4);
- std::unique_ptr<aos::EventLoop> pi4_mapping_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi4_mapping", pi4);
- HandlePiCaptures(pi4_detection_event_loop.get(), pi4_mapping_event_loop.get(),
- &reader, ×tamped_target_detections, &detectors,
- &drawn_nodes, &vis_robot, &display_count, &last_draw_time);
-
- std::unique_ptr<aos::EventLoop> mcap_event_loop;
- std::unique_ptr<aos::McapLogger> relogger;
- if (!FLAGS_mcap_output_path.empty()) {
- LOG(INFO) << "Writing out mcap file to " << FLAGS_mcap_output_path;
- const aos::Node *node =
- aos::configuration::GetNode(reader.configuration(), FLAGS_pi);
- reader.event_loop_factory()->GetNodeEventLoopFactory(node)->OnStartup(
- [&relogger, &mcap_event_loop, &reader, node]() {
- mcap_event_loop =
- reader.event_loop_factory()->MakeEventLoop("mcap", node);
- relogger = std::make_unique<aos::McapLogger>(
- mcap_event_loop.get(), FLAGS_mcap_output_path,
- aos::McapLogger::Serialization::kFlatbuffer,
- aos::McapLogger::CanonicalChannelNames::kShortened,
- aos::McapLogger::Compression::kLz4);
- });
- }
-
- if (FLAGS_visualize) {
- vis_robot.ClearImage();
- const double kFocalLength = 500.0;
- vis_robot.SetDefaultViewpoint(kImageWidth, kFocalLength);
- }
-
+ TargetMapperReplay mapper_replay(&reader);
reader.event_loop_factory()->Run();
-
- if (FLAGS_solve) {
- auto target_constraints =
- DataAdapter::MatchTargetDetections(timestamped_target_detections);
-
- frc971::vision::TargetMapper mapper(FLAGS_json_path, target_constraints);
- mapper.Solve(FLAGS_field_name, FLAGS_output_dir);
- }
-
- // Clean up all the pointers
- for (auto &detector_ptr : detectors) {
- detector_ptr.reset();
- }
+ mapper_replay.MaybeSolve();
}
} // namespace vision
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index 2888690..13e48dc 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -459,7 +459,6 @@
{
"name": "can_logger",
"executable_name": "can_logger",
- "autostart": false,
"nodes": [
"roborio"
]