Merge "Reject to send NANs from the drivetrain code"
diff --git a/WORKSPACE b/WORKSPACE
index f411682..376b91d 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -580,6 +580,13 @@
urls = ["https://www.frc971.org/Build-Dependencies/test_image_frc971.vision.CameraImage_2023.01.28.tar.gz"],
)
+http_file(
+ name = "game_pieces_edgetpu_model",
+ downloaded_file_path = "edgetpu_model.tflite",
+ sha256 = "3d37f34805d017153064076519aaf4b532658a3b8f2518bce8787f27a5c3064c",
+ urls = ["https://www.frc971.org/Build-Dependencies/models/2023/model_edgetpu_2023.04.09_3.tflite"],
+)
+
# Recompressed from libusb-1.0.21.7z.
http_file(
name = "libusb_1_0_windows",
@@ -1567,17 +1574,17 @@
http_archive(
name = "libedgetpu",
build_file = "//third_party:libedgetpu/libedgetpu.BUILD",
- sha256 = "d082df79a33309f58da697cce258acca96ceb12db40660fdbf7826289e4a037c",
+ sha256 = "c900faf2c9ea9599fda60c3d03ac43d0d7b34119659c9e35638b81cd14354b57",
strip_prefix = "libedgetpu-bazel",
- url = "https://www.frc971.org/Build-Dependencies/libedgetpu-1.0.tar.gz",
+ url = "https://www.frc971.org/Build-Dependencies/libedgetpu-ddfa7bde33c23afd8c2892182faa3e5b4e6ad94e.tar.gz",
)
http_archive(
name = "libtensorflowlite",
build_file = "//third_party:libtensorflowlite/libtensorflowlite.BUILD",
- sha256 = "0e3f8deac9c7cdf9aa5812ad6a87af318ed1cf08cb0c414aa494846b7fc15302",
+ sha256 = "a073dfddb3cb25113ba7eac6edb5569d0ae7988cad881d3f665e8ca0b8b85108",
strip_prefix = "tensorflow-bazel",
- url = "https://www.frc971.org/Build-Dependencies/tensorflow-2.8.0.tar.gz",
+ url = "https://www.frc971.org/Build-Dependencies/tensorflow-a4dfb8d1a71385bd6d122e4f27f86dcebb96712d.tar.gz",
)
http_archive(
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_reader.cc b/aos/events/logging/log_reader.cc
index fa5e5d5..0af69c7 100644
--- a/aos/events/logging/log_reader.cc
+++ b/aos/events/logging/log_reader.cc
@@ -1331,45 +1331,8 @@
std::string_view add_prefix,
std::string_view new_type,
RemapConflict conflict_handling) {
- if (replay_channels_ != nullptr) {
- CHECK(std::find(replay_channels_->begin(), replay_channels_->end(),
- std::make_pair(std::string{name}, std::string{type})) !=
- replay_channels_->end())
- << "Attempted to remap channel " << name << " " << type
- << " which is not included in the replay channels passed to LogReader.";
- }
-
- for (size_t ii = 0; ii < logged_configuration()->channels()->size(); ++ii) {
- const Channel *const channel = logged_configuration()->channels()->Get(ii);
- if (channel->name()->str() == name &&
- channel->type()->string_view() == type) {
- CHECK_EQ(0u, remapped_channels_.count(ii))
- << "Already remapped channel "
- << configuration::CleanedChannelToString(channel);
- RemappedChannel remapped_channel;
- remapped_channel.remapped_name =
- std::string(add_prefix) + std::string(name);
- remapped_channel.new_type = new_type;
- const std::string_view remapped_type =
- remapped_channel.new_type.empty() ? type : remapped_channel.new_type;
- CheckAndHandleRemapConflict(
- remapped_channel.remapped_name, remapped_type,
- remapped_configuration_, conflict_handling,
- [this, &remapped_channel, remapped_type, add_prefix,
- conflict_handling]() {
- RemapLoggedChannel(remapped_channel.remapped_name, remapped_type,
- add_prefix, "", conflict_handling);
- });
- remapped_channels_[ii] = std::move(remapped_channel);
- VLOG(1) << "Remapping channel "
- << configuration::CleanedChannelToString(channel)
- << " to have name " << remapped_channels_[ii].remapped_name;
- MakeRemappedConfig();
- return;
- }
- }
- LOG(FATAL) << "Unabled to locate channel with name " << name << " and type "
- << type;
+ RemapLoggedChannel(name, type, nullptr, add_prefix, new_type,
+ conflict_handling);
}
void LogReader::RemapLoggedChannel(std::string_view name, std::string_view type,
@@ -1377,7 +1340,16 @@
std::string_view add_prefix,
std::string_view new_type,
RemapConflict conflict_handling) {
- VLOG(1) << "Node is " << aos::FlatbufferToJson(node);
+ if (node != nullptr) {
+ VLOG(1) << "Node is " << aos::FlatbufferToJson(node);
+ }
+ if (replay_channels_ != nullptr) {
+ CHECK(std::find(replay_channels_->begin(), replay_channels_->end(),
+ std::make_pair(std::string{name}, std::string{type})) !=
+ replay_channels_->end())
+ << "Attempted to remap channel " << name << " " << type
+ << " which is not included in the replay channels passed to LogReader.";
+ }
const Channel *remapped_channel =
configuration::GetChannel(logged_configuration(), name, type, "", node);
CHECK(remapped_channel != nullptr) << ": Failed to find {\"name\": \"" << name
@@ -1408,6 +1380,7 @@
maps_.emplace_back(std::move(new_map));
}
+ // Then remap the logged channel to the prefixed channel.
const size_t channel_index =
configuration::ChannelIndex(logged_configuration(), remapped_channel);
CHECK_EQ(0u, remapped_channels_.count(channel_index))
@@ -1432,6 +1405,51 @@
MakeRemappedConfig();
}
+void LogReader::RenameLoggedChannel(const std::string_view name,
+ const std::string_view type,
+ const std::string_view new_name,
+ const std::vector<MapT> &add_maps) {
+ RenameLoggedChannel(name, type, nullptr, new_name, add_maps);
+}
+
+void LogReader::RenameLoggedChannel(const std::string_view name,
+ const std::string_view type,
+ const Node *const node,
+ const std::string_view new_name,
+ const std::vector<MapT> &add_maps) {
+ if (node != nullptr) {
+ VLOG(1) << "Node is " << aos::FlatbufferToJson(node);
+ }
+ // First find the channel and rename it.
+ const Channel *remapped_channel =
+ configuration::GetChannel(logged_configuration(), name, type, "", node);
+ CHECK(remapped_channel != nullptr) << ": Failed to find {\"name\": \"" << name
+ << "\", \"type\": \"" << type << "\"}";
+ VLOG(1) << "Original {\"name\": \"" << name << "\", \"type\": \"" << type
+ << "\"}";
+ VLOG(1) << "Remapped "
+ << aos::configuration::StrippedChannelToString(remapped_channel);
+
+ const size_t channel_index =
+ configuration::ChannelIndex(logged_configuration(), remapped_channel);
+ CHECK_EQ(0u, remapped_channels_.count(channel_index))
+ << "Already remapped channel "
+ << configuration::CleanedChannelToString(remapped_channel);
+
+ RemappedChannel remapped_channel_struct;
+ remapped_channel_struct.remapped_name = new_name;
+ remapped_channel_struct.new_type.clear();
+ remapped_channels_[channel_index] = std::move(remapped_channel_struct);
+
+ // Then add any provided maps.
+ for (const MapT &map : add_maps) {
+ maps_.push_back(map);
+ }
+
+ // Finally rewrite the config.
+ MakeRemappedConfig();
+}
+
void LogReader::MakeRemappedConfig() {
for (std::unique_ptr<State> &state : states_) {
if (state) {
@@ -1589,20 +1607,26 @@
// Now create the new maps. These are second so they take effect first.
for (const MapT &map : maps_) {
+ CHECK(!map.match->name.empty());
const flatbuffers::Offset<flatbuffers::String> match_name_offset =
fbb.CreateString(map.match->name);
- const flatbuffers::Offset<flatbuffers::String> match_type_offset =
- fbb.CreateString(map.match->type);
- const flatbuffers::Offset<flatbuffers::String> rename_name_offset =
- fbb.CreateString(map.rename->name);
+ flatbuffers::Offset<flatbuffers::String> match_type_offset;
+ if (!map.match->type.empty()) {
+ match_type_offset = fbb.CreateString(map.match->type);
+ }
flatbuffers::Offset<flatbuffers::String> match_source_node_offset;
if (!map.match->source_node.empty()) {
match_source_node_offset = fbb.CreateString(map.match->source_node);
}
+ CHECK(!map.rename->name.empty());
+ const flatbuffers::Offset<flatbuffers::String> rename_name_offset =
+ fbb.CreateString(map.rename->name);
Channel::Builder match_builder(fbb);
match_builder.add_name(match_name_offset);
- match_builder.add_type(match_type_offset);
- if (!map.match->source_node.empty()) {
+ if (!match_type_offset.IsNull()) {
+ match_builder.add_type(match_type_offset);
+ }
+ if (!match_source_node_offset.IsNull()) {
match_builder.add_source_node(match_source_node_offset);
}
const flatbuffers::Offset<Channel> match_offset = match_builder.Finish();
diff --git a/aos/events/logging/log_reader.h b/aos/events/logging/log_reader.h
index d4936f1..6a0fcea 100644
--- a/aos/events/logging/log_reader.h
+++ b/aos/events/logging/log_reader.h
@@ -32,8 +32,7 @@
class EventNotifier;
// Vector of pair of name and type of the channel
-using ReplayChannels =
- std::vector<std::pair<std::string, std::string>>;
+using ReplayChannels = std::vector<std::pair<std::string, std::string>>;
// Vector of channel indices
using ReplayChannelIndices = std::vector<size_t>;
@@ -198,7 +197,6 @@
RemapLoggedChannel(name, T::GetFullyQualifiedName(), add_prefix, new_type,
conflict_handling);
}
-
// Remaps the provided channel, though this respects node mappings, and
// preserves them too. This makes it so if /aos -> /pi1/aos on one node,
// /original/aos -> /original/pi1/aos on the same node after renaming, just
@@ -221,11 +219,40 @@
new_type, conflict_handling);
}
+ // Similar to RemapLoggedChannel(), but lets you specify a name for the new
+ // channel without constraints. This is useful when an application has been
+ // updated to use new channels but you want to support replaying old logs. By
+ // default, this will not add any maps for the new channel. Use add_maps to
+ // specify any maps you'd like added.
+ void RenameLoggedChannel(std::string_view name, std::string_view type,
+ std::string_view new_name,
+ const std::vector<MapT> &add_maps = {});
+ template <typename T>
+ void RenameLoggedChannel(std::string_view name, std::string_view new_name,
+ const std::vector<MapT> &add_maps = {}) {
+ RenameLoggedChannel(name, T::GetFullyQualifiedName(), new_name, add_maps);
+ }
+ // The following overloads are more suitable for multi-node configurations,
+ // and let you rename a channel on a specific node.
+ void RenameLoggedChannel(std::string_view name, std::string_view type,
+ const Node *node, std::string_view new_name,
+ const std::vector<MapT> &add_maps = {});
+ template <typename T>
+ void RenameLoggedChannel(std::string_view name, const Node *node,
+ std::string_view new_name,
+ const std::vector<MapT> &add_maps = {}) {
+ RenameLoggedChannel(name, T::GetFullyQualifiedName(), node, new_name,
+ add_maps);
+ }
+
template <typename T>
bool HasChannel(std::string_view name, const Node *node = nullptr) {
- return configuration::GetChannel(logged_configuration(), name,
- T::GetFullyQualifiedName(), "", node,
- true) != nullptr;
+ return HasChannel(name, T::GetFullyQualifiedName(), node);
+ }
+ bool HasChannel(std::string_view name, std::string_view type,
+ const Node *node) {
+ return configuration::GetChannel(logged_configuration(), name, type, "",
+ node, true) != nullptr;
}
template <typename T>
@@ -235,6 +262,14 @@
RemapLoggedChannel<T>(name, node);
}
}
+ template <typename T>
+ void MaybeRenameLoggedChannel(std::string_view name, const Node *node,
+ std::string_view new_name,
+ const std::vector<MapT> &add_maps = {}) {
+ if (HasChannel<T>(name, node)) {
+ RenameLoggedChannel<T>(name, node, new_name, add_maps);
+ }
+ }
// Returns true if the channel exists on the node and was logged.
template <typename T>
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..7214a36 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,13 +90,12 @@
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());
- // Confirm that we can remap logged channels to point to new buses.
- reader.RemapLoggedChannel<aos::examples::Ping>("/test", "/original");
-
// This sends out the fetched messages and advances time to the start of the
// log file.
reader.Register();
@@ -107,8 +108,8 @@
int ping_count = 10;
int pong_count = 10;
- // Confirm that the ping value matches in the remapped channel location.
- test_event_loop->MakeWatcher("/original/test",
+ // Confirm that the ping value matches.
+ test_event_loop->MakeWatcher("/test",
[&ping_count](const examples::Ping &ping) {
EXPECT_EQ(ping.value(), ping_count + 1);
++ping_count;
@@ -124,6 +125,7 @@
reader.event_loop_factory()->RunFor(std::chrono::seconds(100));
EXPECT_EQ(ping_count, 2010);
+ EXPECT_EQ(pong_count, ping_count);
}
// Tests calling StartLogging twice.
@@ -152,16 +154,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 +231,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 +269,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.cc b/aos/events/logging/multinode_logger_test.cc
index f1784bf..18337be 100644
--- a/aos/events/logging/multinode_logger_test.cc
+++ b/aos/events/logging/multinode_logger_test.cc
@@ -166,10 +166,12 @@
UnorderedElementsAre(
std::make_tuple("/pi1/aos",
"aos.message_bridge.ServerStatistics", 1),
- std::make_tuple("/test", "aos.examples.Ping", 1)))
+ std::make_tuple("/test", "aos.examples.Ping", 1),
+ std::make_tuple("/pi1/aos", "aos.examples.Ping", 1)))
<< " : " << logfiles_[2];
{
std::vector<std::tuple<std::string, std::string, int>> channel_counts = {
+ std::make_tuple("/pi1/aos", "aos.examples.Ping", 10),
std::make_tuple("/pi1/aos", "aos.message_bridge.Timestamp", 1),
std::make_tuple("/pi1/aos", "aos.message_bridge.ClientStatistics",
1)};
@@ -185,12 +187,13 @@
}
{
std::vector<std::tuple<std::string, std::string, int>> channel_counts = {
+ std::make_tuple("/pi1/aos", "aos.examples.Ping", 1990),
std::make_tuple("/pi1/aos", "aos.message_bridge.Timestamp", 199),
std::make_tuple("/pi1/aos", "aos.message_bridge.ServerStatistics",
20),
std::make_tuple("/pi1/aos", "aos.message_bridge.ClientStatistics",
199),
- std::make_tuple("/pi1/aos", "aos.timing.Report", 40),
+ std::make_tuple("/pi1/aos", "aos.timing.Report", 60),
std::make_tuple("/test", "aos.examples.Ping", 2000)};
if (!std::get<0>(GetParam()).shared) {
channel_counts.push_back(
@@ -237,8 +240,10 @@
// Timing reports and pongs.
EXPECT_THAT(CountChannelsData(config, logfiles_[7]),
- UnorderedElementsAre(std::make_tuple(
- "/pi2/aos", "aos.message_bridge.ServerStatistics", 1)))
+ UnorderedElementsAre(
+ std::make_tuple("/pi2/aos", "aos.examples.Ping", 1),
+ std::make_tuple("/pi2/aos",
+ "aos.message_bridge.ServerStatistics", 1)))
<< " : " << logfiles_[7];
EXPECT_THAT(
CountChannelsData(config, logfiles_[8]),
@@ -247,12 +252,13 @@
EXPECT_THAT(
CountChannelsData(config, logfiles_[9]),
UnorderedElementsAre(
+ std::make_tuple("/pi2/aos", "aos.examples.Ping", 2000),
std::make_tuple("/pi2/aos", "aos.message_bridge.Timestamp", 200),
std::make_tuple("/pi2/aos", "aos.message_bridge.ServerStatistics",
20),
std::make_tuple("/pi2/aos", "aos.message_bridge.ClientStatistics",
200),
- std::make_tuple("/pi2/aos", "aos.timing.Report", 40),
+ std::make_tuple("/pi2/aos", "aos.timing.Report", 60),
std::make_tuple("/test", "aos.examples.Pong", 2000)))
<< " : " << logfiles_[9];
// And ping timestamps.
@@ -946,7 +952,7 @@
VerifyParts(sorted_parts);
}
-// Tests that if we remap a remapped channel, it shows up correctly.
+// Tests that if we remap a logged channel, it shows up correctly.
TEST_P(MultinodeLoggerTest, RemapLoggedChannel) {
time_converter_.StartEqual();
{
@@ -1025,6 +1031,117 @@
reader.Deregister();
}
+// Tests that if we rename a logged channel, it shows up correctly.
+TEST_P(MultinodeLoggerTest, RenameLoggedChannel) {
+ std::vector<std::string> actual_filenames;
+ time_converter_.StartEqual();
+ {
+ LoggerState pi1_logger = MakeLogger(pi1_);
+ LoggerState pi2_logger = MakeLogger(pi2_);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(95));
+
+ StartLogger(&pi1_logger);
+ StartLogger(&pi2_logger);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(20000));
+
+ pi1_logger.AppendAllFilenames(&actual_filenames);
+ pi2_logger.AppendAllFilenames(&actual_filenames);
+ }
+
+ LogReader reader(SortParts(actual_filenames));
+
+ // Rename just on pi2. Add some global maps just to verify they get added in
+ // the config and used correctly.
+ std::vector<MapT> maps;
+ {
+ MapT map;
+ map.match = std::make_unique<ChannelT>();
+ map.match->name = "/foo*";
+ map.match->source_node = "pi1";
+ map.rename = std::make_unique<ChannelT>();
+ map.rename->name = "/pi1/foo";
+ maps.emplace_back(std::move(map));
+ }
+ {
+ MapT map;
+ map.match = std::make_unique<ChannelT>();
+ map.match->name = "/foo*";
+ map.match->source_node = "pi2";
+ map.rename = std::make_unique<ChannelT>();
+ map.rename->name = "/pi2/foo";
+ maps.emplace_back(std::move(map));
+ }
+ {
+ MapT map;
+ map.match = std::make_unique<ChannelT>();
+ map.match->name = "/foo";
+ map.match->type = "aos.examples.Ping";
+ map.rename = std::make_unique<ChannelT>();
+ map.rename->name = "/foo/renamed";
+ maps.emplace_back(std::move(map));
+ }
+ reader.RenameLoggedChannel<aos::examples::Ping>(
+ "/aos", configuration::GetNode(reader.configuration(), "pi2"),
+ "/pi2/foo/renamed", maps);
+
+ SimulatedEventLoopFactory log_reader_factory(reader.configuration());
+ log_reader_factory.set_send_delay(chrono::microseconds(0));
+
+ std::vector<const Channel *> remapped_channels = reader.RemappedChannels();
+ // Note: An extra channel gets remapped automatically due to a timestamp
+ // channel being LOCAL_LOGGER'd.
+ const bool shared = std::get<0>(GetParam()).shared;
+ ASSERT_EQ(remapped_channels.size(), shared ? 1u : 2u);
+ EXPECT_EQ(remapped_channels[shared ? 0 : 1]->name()->string_view(),
+ "/pi2/foo/renamed");
+ EXPECT_EQ(remapped_channels[shared ? 0 : 1]->type()->string_view(),
+ "aos.examples.Ping");
+ if (!shared) {
+ EXPECT_EQ(remapped_channels[0]->name()->string_view(),
+ "/original/pi1/aos/remote_timestamps/pi2/pi1/aos/"
+ "aos-message_bridge-Timestamp");
+ EXPECT_EQ(remapped_channels[0]->type()->string_view(),
+ "aos.message_bridge.RemoteMessage");
+ }
+
+ reader.Register(&log_reader_factory);
+
+ const Node *pi1 =
+ configuration::GetNode(log_reader_factory.configuration(), "pi1");
+ const Node *pi2 =
+ configuration::GetNode(log_reader_factory.configuration(), "pi2");
+
+ // Confirm we can read the data on the renamed channel, just for pi2. Nothing
+ // else should have moved.
+ std::unique_ptr<EventLoop> pi2_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi2);
+ pi2_event_loop->SkipTimingReport();
+ std::unique_ptr<EventLoop> full_pi2_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi2);
+ full_pi2_event_loop->SkipTimingReport();
+ std::unique_ptr<EventLoop> pi1_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi1);
+ pi1_event_loop->SkipTimingReport();
+
+ MessageCounter<aos::examples::Ping> pi2_ping(pi2_event_loop.get(), "/aos");
+ MessageCounter<aos::examples::Ping> pi2_renamed_ping(pi2_event_loop.get(),
+ "/foo");
+ MessageCounter<aos::examples::Ping> full_pi2_renamed_ping(
+ full_pi2_event_loop.get(), "/pi2/foo/renamed");
+ MessageCounter<aos::examples::Ping> pi1_ping(pi1_event_loop.get(), "/aos");
+
+ log_reader_factory.Run();
+
+ EXPECT_EQ(pi2_ping.count(), 0u);
+ EXPECT_NE(pi2_renamed_ping.count(), 0u);
+ EXPECT_NE(full_pi2_renamed_ping.count(), 0u);
+ EXPECT_NE(pi1_ping.count(), 0u);
+
+ reader.Deregister();
+}
+
// Tests that we can remap a forwarded channel as well.
TEST_P(MultinodeLoggerTest, RemapForwardedLoggedChannel) {
time_converter_.StartEqual();
@@ -1102,6 +1219,109 @@
reader.Deregister();
}
+// Tests that we can rename a forwarded channel as well.
+TEST_P(MultinodeLoggerTest, RenameForwardedLoggedChannel) {
+ std::vector<std::string> actual_filenames;
+ time_converter_.StartEqual();
+ {
+ LoggerState pi1_logger = MakeLogger(pi1_);
+ LoggerState pi2_logger = MakeLogger(pi2_);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(95));
+
+ StartLogger(&pi1_logger);
+ StartLogger(&pi2_logger);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(20000));
+
+ pi1_logger.AppendAllFilenames(&actual_filenames);
+ pi2_logger.AppendAllFilenames(&actual_filenames);
+ }
+
+ LogReader reader(SortParts(actual_filenames));
+
+ std::vector<MapT> maps;
+ {
+ MapT map;
+ map.match = std::make_unique<ChannelT>();
+ map.match->name = "/production*";
+ map.match->source_node = "pi1";
+ map.rename = std::make_unique<ChannelT>();
+ map.rename->name = "/pi1/production";
+ maps.emplace_back(std::move(map));
+ }
+ {
+ MapT map;
+ map.match = std::make_unique<ChannelT>();
+ map.match->name = "/production*";
+ map.match->source_node = "pi2";
+ map.rename = std::make_unique<ChannelT>();
+ map.rename->name = "/pi2/production";
+ maps.emplace_back(std::move(map));
+ }
+ reader.RenameLoggedChannel<aos::examples::Ping>(
+ "/test", configuration::GetNode(reader.configuration(), "pi1"),
+ "/pi1/production", maps);
+
+ SimulatedEventLoopFactory log_reader_factory(reader.configuration());
+ log_reader_factory.set_send_delay(chrono::microseconds(0));
+
+ reader.Register(&log_reader_factory);
+
+ const Node *pi1 =
+ configuration::GetNode(log_reader_factory.configuration(), "pi1");
+ const Node *pi2 =
+ configuration::GetNode(log_reader_factory.configuration(), "pi2");
+
+ // Confirm we can read the data on the renamed channel, on both the source
+ // node and the remote node. In case of split timestamp channels, confirm that
+ // we receive the timestamp messages on the renamed channel as well.
+ std::unique_ptr<EventLoop> pi1_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi1);
+ pi1_event_loop->SkipTimingReport();
+ std::unique_ptr<EventLoop> full_pi1_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi1);
+ full_pi1_event_loop->SkipTimingReport();
+ std::unique_ptr<EventLoop> pi2_event_loop =
+ log_reader_factory.MakeEventLoop("test", pi2);
+ pi2_event_loop->SkipTimingReport();
+
+ MessageCounter<examples::Ping> pi1_ping(pi1_event_loop.get(), "/test");
+ MessageCounter<examples::Ping> pi2_ping(pi2_event_loop.get(), "/test");
+ MessageCounter<examples::Ping> pi1_renamed_ping(pi1_event_loop.get(),
+ "/pi1/production");
+ MessageCounter<examples::Ping> pi2_renamed_ping(pi2_event_loop.get(),
+ "/pi1/production");
+
+ std::unique_ptr<MessageCounter<message_bridge::RemoteMessage>>
+ pi1_renamed_ping_timestamp;
+ std::unique_ptr<MessageCounter<message_bridge::RemoteMessage>>
+ pi1_ping_timestamp;
+ if (!shared()) {
+ pi1_renamed_ping_timestamp =
+ std::make_unique<MessageCounter<message_bridge::RemoteMessage>>(
+ pi1_event_loop.get(),
+ "/pi1/aos/remote_timestamps/pi2/pi1/production/aos-examples-Ping");
+ pi1_ping_timestamp =
+ std::make_unique<MessageCounter<message_bridge::RemoteMessage>>(
+ pi1_event_loop.get(),
+ "/pi1/aos/remote_timestamps/pi2/test/aos-examples-Ping");
+ }
+
+ log_reader_factory.Run();
+
+ EXPECT_EQ(pi1_ping.count(), 0u);
+ EXPECT_EQ(pi2_ping.count(), 0u);
+ EXPECT_NE(pi1_renamed_ping.count(), 0u);
+ EXPECT_NE(pi2_renamed_ping.count(), 0u);
+ if (!shared()) {
+ EXPECT_NE(pi1_renamed_ping_timestamp->count(), 0u);
+ EXPECT_EQ(pi1_ping_timestamp->count(), 0u);
+ }
+
+ reader.Deregister();
+}
+
// Tests that we observe all the same events in log replay (for a given node)
// whether we just register an event loop for that node or if we register a full
// event loop factory.
diff --git a/aos/events/logging/multinode_logger_test_lib.cc b/aos/events/logging/multinode_logger_test_lib.cc
index 0f588c6..22822e7 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);
@@ -115,8 +115,14 @@
LOG(INFO) << "Logging data to " << logfiles_[0] << ", " << logfiles_[1]
<< " and " << logfiles_[2];
- pi1_->OnStartup([this]() { pi1_->AlwaysStart<Ping>("ping"); });
- pi2_->OnStartup([this]() { pi2_->AlwaysStart<Pong>("pong"); });
+ pi1_->OnStartup([this]() {
+ pi1_->AlwaysStart<Ping>("ping");
+ pi1_->AlwaysStart<Ping>("ping_local", "/aos");
+ });
+ pi2_->OnStartup([this]() {
+ pi2_->AlwaysStart<Pong>("pong");
+ pi2_->AlwaysStart<Ping>("ping_local", "/aos");
+ });
}
bool MultinodeLoggerTest::shared() const {
diff --git a/aos/events/logging/multinode_logger_test_lib.h b/aos/events/logging/multinode_logger_test_lib.h
index f3f322e..40b5933 100644
--- a/aos/events/logging/multinode_logger_test_lib.h
+++ b/aos/events/logging/multinode_logger_test_lib.h
@@ -56,13 +56,13 @@
};
constexpr std::string_view kCombinedConfigSha1() {
- return "5d73fe35bacaa59d24f8f0c1a806fe10b783b0fcc80809ee30a9db824e82538b";
+ return "c8cd3762e42a4e19b2155f63ccec97d1627a2fbd34d3da3ea6541128ca22b899";
}
constexpr std::string_view kSplitConfigSha1() {
- return "f25e8f6f90d61f41c41517e652300566228b077e44cd86f1af2af4a9bed31ad4";
+ return "0ee6360b3e82a46f3f8b241661934abac53957d494a81ed1938899c220334954";
}
constexpr std::string_view kReloggedSplitConfigSha1() {
- return "f1fabd629bdf8735c3d81bc791d7a454e8e636951c26cba6426545cbc97f911f";
+ return "cc31e1a644dd7bf65d72247aea3e09b3474753e01921f3b6272f8233f288a16b";
}
LoggerState MakeLoggerState(NodeEventLoopFactory *node,
diff --git a/aos/events/logging/multinode_pingpong_combined.json b/aos/events/logging/multinode_pingpong_combined.json
index 6686360..4864674 100644
--- a/aos/events/logging/multinode_pingpong_combined.json
+++ b/aos/events/logging/multinode_pingpong_combined.json
@@ -112,6 +112,18 @@
"num_senders": 2,
"source_node": "pi2"
},
+ {
+ "name": "/pi1/aos",
+ "type": "aos.examples.Ping",
+ "source_node": "pi1",
+ "frequency": 150
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.examples.Ping",
+ "source_node": "pi2",
+ "frequency": 150
+ },
/* Forwarded to pi2 */
{
"name": "/test",
diff --git a/aos/events/logging/multinode_pingpong_split.json b/aos/events/logging/multinode_pingpong_split.json
index f5bd9cb..4cd6248 100644
--- a/aos/events/logging/multinode_pingpong_split.json
+++ b/aos/events/logging/multinode_pingpong_split.json
@@ -35,6 +35,18 @@
},
{
"name": "/pi1/aos",
+ "type": "aos.examples.Ping",
+ "source_node": "pi1",
+ "frequency": 150
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.examples.Ping",
+ "source_node": "pi2",
+ "frequency": 150
+ },
+ {
+ "name": "/pi1/aos",
"type": "aos.message_bridge.ServerStatistics",
"logger": "LOCAL_LOGGER",
"source_node": "pi1"
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/aos/events/ping_lib.cc b/aos/events/ping_lib.cc
index f9958ff..e80fd75 100644
--- a/aos/events/ping_lib.cc
+++ b/aos/events/ping_lib.cc
@@ -12,14 +12,16 @@
namespace chrono = std::chrono;
-Ping::Ping(EventLoop *event_loop)
+Ping::Ping(EventLoop *event_loop, std::string_view channel_name)
: event_loop_(event_loop),
- sender_(event_loop_->MakeSender<examples::Ping>("/test")) {
+ sender_(event_loop_->MakeSender<examples::Ping>(channel_name)) {
timer_handle_ = event_loop_->AddTimer([this]() { SendPing(); });
timer_handle_->set_name("ping");
- event_loop_->MakeWatcher(
- "/test", [this](const examples::Pong &pong) { HandlePong(pong); });
+ if (event_loop_->HasChannel<examples::Pong>(channel_name)) {
+ event_loop_->MakeWatcher(
+ channel_name, [this](const examples::Pong &pong) { HandlePong(pong); });
+ }
event_loop_->OnRun([this]() {
timer_handle_->Setup(event_loop_->monotonic_now(),
diff --git a/aos/events/ping_lib.h b/aos/events/ping_lib.h
index 0b83d52..fec6c45 100644
--- a/aos/events/ping_lib.h
+++ b/aos/events/ping_lib.h
@@ -2,6 +2,7 @@
#define AOS_EVENTS_PING_LIB_H_
#include <chrono>
+#include <string_view>
#include "aos/events/event_loop.h"
#include "aos/events/ping_generated.h"
@@ -12,7 +13,7 @@
// Class which sends out a Ping message every X ms, and times the response.
class Ping {
public:
- Ping(EventLoop *event_loop);
+ Ping(EventLoop *event_loop, std::string_view channel_name = "/test");
void set_quiet(bool quiet) { quiet_ = quiet; }
diff --git a/aos/logging/log_namer.cc b/aos/logging/log_namer.cc
index dfebe44..ed57caa 100644
--- a/aos/logging/log_namer.cc
+++ b/aos/logging/log_namer.cc
@@ -111,19 +111,19 @@
} // namespace
-std::string GetLogName(const char *basename) {
+std::optional<std::string> MaybeGetLogName(const char *basename) {
if (FLAGS_logging_folder.empty()) {
char folder[128];
{
char dev_name[8];
- while (!FindDevice(dev_name, sizeof(dev_name))) {
+ if (!FindDevice(dev_name, sizeof(dev_name))) {
LOG(INFO) << "Waiting for a device";
- sleep(5);
+ return std::nullopt;
}
snprintf(folder, sizeof(folder), "/media/%s1", dev_name);
- while (!FoundThumbDrive(folder)) {
+ if (!FoundThumbDrive(folder)) {
LOG(INFO) << "Waiting for" << folder;
- sleep(1);
+ return std::nullopt;
}
snprintf(folder, sizeof(folder), "/media/%s1/", dev_name);
}
@@ -164,5 +164,21 @@
return log_base_name;
}
+std::string GetLogName(const char *basename) {
+ std::optional<std::string> log_base_name;
+
+ while (true) {
+ log_base_name = MaybeGetLogName(basename);
+
+ if (log_base_name.has_value()) {
+ break;
+ }
+
+ sleep(5);
+ }
+
+ return log_base_name.value();
+}
+
} // namespace logging
} // namespace aos
diff --git a/aos/logging/log_namer.h b/aos/logging/log_namer.h
index 72abf12..44e93bf 100644
--- a/aos/logging/log_namer.h
+++ b/aos/logging/log_namer.h
@@ -2,6 +2,7 @@
#define AOS_LOGGING_LOG_NAMER_H_
#include <string>
+#include <optional>
namespace aos {
namespace logging {
@@ -13,6 +14,11 @@
// the form "/media/sda1/abc-123" and setup a symlink pointing to it at
// "/media/sda1/abc-current".
std::string GetLogName(const char *basename);
+
+// A nonblocking variant of GetLogName that you can poll instead of blocking for
+// the usb drive.
+std::optional<std::string> MaybeGetLogName(const char *basename);
+
} // namespace logging
} // namespace aos
diff --git a/debian/opencv.BUILD b/debian/opencv.BUILD
index c989e22..dfc3250 100644
--- a/debian/opencv.BUILD
+++ b/debian/opencv.BUILD
@@ -28,6 +28,8 @@
"usr/lib/%s/libopencv_aruco.so.4.5",
"usr/lib/%s/libopencv_imgcodecs.so.4.5",
"usr/lib/%s/libopencv_ml.so.4.5",
+ "usr/lib/%s/libopencv_dnn.so.4.5",
+ "usr/lib/%s/libprotobuf.so.23",
"usr/lib/%s/libopencv_calib3d.so.4.5",
"usr/lib/%s/libtbb.so.2",
"usr/lib/%s/libgtk-3.so.0",
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 90c1454..22909be 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -450,6 +450,35 @@
return false;
}
+bool BaseAutonomousActor::SplineHandle::SplineDistanceTraveled(
+ double distance) {
+ base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
+ if (base_autonomous_actor_->drivetrain_status_fetcher_.get()) {
+ // Confirm that:
+ // (a) The spline has started executiong (is_executing remains true even
+ // when we reach the end of the spline).
+ // (b) The spline that we are executing is the correct one.
+ // (c) There is less than distance distance remaining.
+ if (base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->goal_spline_handle() != spline_handle_) {
+ // Never done if we aren't the active spline.
+ return false;
+ }
+
+ if (base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging()
+ ->is_executed()) {
+ return true;
+ }
+ return base_autonomous_actor_->drivetrain_status_fetcher_
+ ->trajectory_logging()
+ ->is_executing() &&
+ base_autonomous_actor_->drivetrain_status_fetcher_
+ ->trajectory_logging()
+ ->distance_traveled() > distance;
+ }
+ return false;
+}
+
bool BaseAutonomousActor::SplineHandle::WaitForSplineDistanceRemaining(
double distance) {
::aos::time::PhasedLoop phased_loop(
@@ -467,6 +496,23 @@
}
}
+bool BaseAutonomousActor::SplineHandle::WaitForSplineDistanceTraveled(
+ double distance) {
+ ::aos::time::PhasedLoop phased_loop(
+ frc971::controls::kLoopFrequency,
+ base_autonomous_actor_->event_loop()->monotonic_now(),
+ ActorBase::kLoopOffset);
+ while (true) {
+ if (base_autonomous_actor_->ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ if (SplineDistanceTraveled(distance)) {
+ return true;
+ }
+ }
+}
+
void BaseAutonomousActor::LineFollowAtVelocity(
double velocity, y2019::control_loops::drivetrain::SelectionHint hint) {
{
diff --git a/frc971/autonomous/base_autonomous_actor.h b/frc971/autonomous/base_autonomous_actor.h
index 5562dde..403854f 100644
--- a/frc971/autonomous/base_autonomous_actor.h
+++ b/frc971/autonomous/base_autonomous_actor.h
@@ -41,7 +41,9 @@
// Whether there is less than a certain distance, in meters, remaining in
// the current spline.
bool SplineDistanceRemaining(double distance);
+ bool SplineDistanceTraveled(double distance);
bool WaitForSplineDistanceRemaining(double distance);
+ bool WaitForSplineDistanceTraveled(double distance);
// Returns [x, y, theta] position of the start.
const Eigen::Vector3d &starting_position() const { return spline_start_; }
diff --git a/frc971/can_logger/BUILD b/frc971/can_logger/BUILD
index a3b002a..a18a379 100644
--- a/frc971/can_logger/BUILD
+++ b/frc971/can_logger/BUILD
@@ -37,3 +37,36 @@
gen_reflections = 1,
visibility = ["//visibility:public"],
)
+
+cc_binary(
+ name = "log_to_asc",
+ srcs = [
+ "log_to_asc.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":asc_logger",
+ ":can_logging_fbs",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events/logging:log_reader",
+ "//aos/time",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_library(
+ name = "asc_logger",
+ srcs = [
+ "asc_logger.cc",
+ ],
+ hdrs = [
+ "asc_logger.h",
+ ],
+ deps = [
+ ":can_logging_fbs",
+ "//aos/events:event_loop",
+ "//aos/scoped:scoped_fd",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/frc971/can_logger/asc_logger.cc b/frc971/can_logger/asc_logger.cc
new file mode 100644
index 0000000..96b2e96
--- /dev/null
+++ b/frc971/can_logger/asc_logger.cc
@@ -0,0 +1,142 @@
+#include "frc971/can_logger/asc_logger.h"
+
+#include <linux/can.h>
+
+namespace frc971 {
+namespace can_logger {
+
+AscLogger::AscLogger(aos::EventLoop *event_loop, const std::string &filename)
+ : output_(filename), event_loop_(event_loop) {
+ CHECK(output_);
+ event_loop->MakeWatcher(
+ "/can", [this](const CanFrame &frame) { HandleFrame(frame); });
+}
+
+void AscLogger::HandleFrame(const CanFrame &frame) {
+ if (!first_frame_monotonic_) {
+ aos::monotonic_clock::time_point time(
+ std::chrono::nanoseconds(frame.monotonic_timestamp_ns()));
+
+ first_frame_monotonic_ = time;
+
+ WriteHeader(output_, event_loop_->realtime_now());
+ }
+
+ WriteFrame(output_, frame);
+}
+
+void AscLogger::WriteHeader(std::ostream &file,
+ aos::realtime_clock::time_point start_time) {
+ file << "date " << start_time << "\n";
+ file << "base hex timetamps absolute\n";
+ file << "no internal events logged\n";
+}
+
+namespace {
+
+static const unsigned char len2dlc[] = {
+ 0, 1, 2, 3, 4, 5, 6, 7, 8, /* 0 - 8 */
+ 9, 9, 9, 9, /* 9 - 12 */
+ 10, 10, 10, 10, /* 13 - 16 */
+ 11, 11, 11, 11, /* 17 - 20 */
+ 12, 12, 12, 12, /* 21 - 24 */
+ 13, 13, 13, 13, 13, 13, 13, 13, /* 25 - 32 */
+ 14, 14, 14, 14, 14, 14, 14, 14, /* 33 - 40 */
+ 14, 14, 14, 14, 14, 14, 14, 14, /* 41 - 48 */
+ 15, 15, 15, 15, 15, 15, 15, 15, /* 49 - 56 */
+ 15, 15, 15, 15, 15, 15, 15, 15}; /* 57 - 64 */
+
+/* map the sanitized data length to an appropriate data length code */
+unsigned char can_fd_len2dlc(unsigned char len) {
+ if (len > 64) return 0xF;
+
+ return len2dlc[len];
+}
+
+#define ASC_F_RTR 0x00000010
+#define ASC_F_FDF 0x00001000
+#define ASC_F_BRS 0x00002000
+#define ASC_F_ESI 0x00004000
+
+} // namespace
+
+void AscLogger::WriteFrame(std::ostream &file, const CanFrame &frame) {
+ aos::monotonic_clock::time_point frame_timestamp(
+ std::chrono::nanoseconds(frame.monotonic_timestamp_ns()));
+
+ std::chrono::duration<double> time(frame_timestamp -
+ first_frame_monotonic_.value());
+
+ // TODO: maybe this should not be hardcoded
+ const int device_id = 1;
+
+ // EFF/SFF is set in the MSB
+ bool is_extended_frame_format = frame.can_id() & CAN_EFF_FLAG;
+
+ uint32_t id_mask = is_extended_frame_format ? CAN_EFF_MASK : CAN_SFF_MASK;
+ int id = frame.can_id() & id_mask;
+
+ // data length code
+ int dlc = can_fd_len2dlc(frame.data()->size());
+
+ const uint8_t flags = frame.flags();
+
+ uint32_t asc_flags = 0;
+
+ // Mark it as a CAN FD Frame
+ asc_flags = ASC_F_FDF;
+
+ // Pass through the bit rate switch flag
+ // indicates that it used a second bitrate for payload data
+ if (flags & CANFD_BRS) {
+ asc_flags |= ASC_F_BRS;
+ }
+
+ // ESI is the error state indicator of the transmitting node
+ if (flags & CANFD_ESI) {
+ asc_flags |= ASC_F_ESI;
+ }
+
+ file << std::fixed << time.count() << " ";
+
+ file << "CANFD ";
+
+ file << std::setfill(' ') << std::setw(3) << std::right << device_id << " ";
+
+ file << "Rx ";
+
+ std::stringstream formatted_id;
+ formatted_id << std::hex << std::uppercase << std::setfill('0') << id
+ << std::dec;
+ if (is_extended_frame_format) {
+ formatted_id << "x";
+ }
+
+ file << std::setfill(' ') << std::setw(11) << formatted_id.str();
+ file << " ";
+
+ file << ((flags & CANFD_BRS) ? '1' : '0') << " ";
+ file << ((flags & CANFD_ESI) ? '1' : '0') << " ";
+
+ file << std::hex << std::nouppercase << dlc << std::dec << " ";
+
+ // actual data length
+ file << std::setfill(' ') << std::setw(2) << frame.data()->size();
+
+ file << std::hex << std::uppercase;
+ for (uint8_t byte : *frame.data()) {
+ file << " " << std::setfill('0') << std::setw(2) << static_cast<int>(byte);
+ }
+ file << std::dec;
+
+ // these are hardcoded in log2asc too, I don't know why
+ file << " 130000 130 ";
+ file << std::setfill(' ') << std::setw(8) << std::hex << asc_flags
+ << std::dec;
+ file << " 0 0 0 0 0";
+
+ file << "\n";
+}
+
+} // namespace can_logger
+} // namespace frc971
diff --git a/frc971/can_logger/asc_logger.h b/frc971/can_logger/asc_logger.h
new file mode 100644
index 0000000..213aa25
--- /dev/null
+++ b/frc971/can_logger/asc_logger.h
@@ -0,0 +1,38 @@
+#ifndef FRC971_CAN_LOGGER_ASC_LOGGER_H_
+#define FRC971_CAN_LOGGER_ASC_LOGGER_H_
+
+#include <iomanip>
+#include <iostream>
+
+#include "aos/events/event_loop.h"
+#include "frc971/can_logger/can_logging_generated.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+namespace frc971 {
+namespace can_logger {
+
+class AscLogger {
+ public:
+ AscLogger(aos::EventLoop *event_loop, const std::string &filename);
+
+ private:
+ void HandleFrame(const CanFrame &frame);
+
+ // This implementation attempts to duplicate the output of can-utils/log2asc
+ void WriteFrame(std::ostream &file, const CanFrame &frame);
+
+ static void WriteHeader(std::ostream &file,
+ aos::realtime_clock::time_point start_time);
+
+ std::optional<aos::monotonic_clock::time_point> first_frame_monotonic_;
+
+ std::ofstream output_;
+
+ aos::EventLoop *event_loop_;
+};
+
+} // namespace can_logger
+} // namespace frc971
+
+#endif // FRC971_CAN_LOGGER_ASC_LOGGER_H_
diff --git a/frc971/can_logger/can_logger.cc b/frc971/can_logger/can_logger.cc
index 6b4258a..ccd9824 100644
--- a/frc971/can_logger/can_logger.cc
+++ b/frc971/can_logger/can_logger.cc
@@ -72,6 +72,7 @@
CanFrame::Builder can_frame_builder = builder.MakeBuilder<CanFrame>();
can_frame_builder.add_can_id(frame.can_id);
+ can_frame_builder.add_flags(frame.flags);
can_frame_builder.add_data(frame_data);
can_frame_builder.add_monotonic_timestamp_ns(
static_cast<std::chrono::nanoseconds>(
diff --git a/frc971/can_logger/can_logging.fbs b/frc971/can_logger/can_logging.fbs
index d6ec8b9..ba60241 100644
--- a/frc971/can_logger/can_logging.fbs
+++ b/frc971/can_logger/can_logging.fbs
@@ -8,6 +8,8 @@
data:[ubyte] (id: 1);
// The hardware timestamp of when the frame came in
monotonic_timestamp_ns:uint64 (id: 2);
+ // Additional flags for CAN FD
+ flags: ubyte (id: 3);
}
root_type CanFrame;
diff --git a/frc971/can_logger/log_to_asc.cc b/frc971/can_logger/log_to_asc.cc
new file mode 100644
index 0000000..73ddd4e
--- /dev/null
+++ b/frc971/can_logger/log_to_asc.cc
@@ -0,0 +1,57 @@
+#include "aos/configuration.h"
+#include "aos/events/event_loop_generated.h"
+#include "aos/events/logging/log_reader.h"
+#include "aos/init.h"
+#include "frc971/can_logger/asc_logger.h"
+#include "frc971/can_logger/can_logging_generated.h"
+
+DEFINE_string(node, "", "Node to replay from the perspective of.");
+DEFINE_string(output_path, "/tmp/can_log.asc", "Log to output.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ const std::vector<aos::logger::LogFile> logfiles =
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv));
+ CHECK(!logfiles.empty());
+ const std::string logger_node = logfiles.at(0).logger_node;
+ bool all_logs_from_same_node = true;
+ for (const aos::logger::LogFile &log : logfiles) {
+ if (log.logger_node != logger_node) {
+ all_logs_from_same_node = false;
+ break;
+ }
+ }
+ std::string replay_node = FLAGS_node;
+ if (replay_node.empty() && all_logs_from_same_node) {
+ LOG(INFO) << "Guessing \"" << logger_node
+ << "\" as node given that --node was not specified.";
+ replay_node = logger_node;
+ }
+
+ std::optional<aos::FlatbufferDetachedBuffer<aos::Configuration>> config;
+
+ aos::logger::LogReader reader(
+ logfiles, config.has_value() ? &config.value().message() : nullptr);
+ aos::SimulatedEventLoopFactory factory(reader.configuration());
+ reader.RegisterWithoutStarting(&factory);
+
+ const aos::Node *node =
+ (replay_node.empty() ||
+ !aos::configuration::MultiNode(reader.configuration()))
+ ? nullptr
+ : aos::configuration::GetNode(reader.configuration(), replay_node);
+
+ std::unique_ptr<aos::EventLoop> can_event_loop;
+ CHECK(!FLAGS_output_path.empty());
+ std::unique_ptr<frc971::can_logger::AscLogger> relogger;
+
+ factory.GetNodeEventLoopFactory(node)->OnStartup([&relogger, &can_event_loop,
+ &reader, node]() {
+ can_event_loop = reader.event_loop_factory()->MakeEventLoop("can", node);
+ relogger = std::make_unique<frc971::can_logger::AscLogger>(
+ can_event_loop.get(), FLAGS_output_path);
+ });
+ reader.event_loop_factory()->Run();
+ reader.Deregister();
+}
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index a1f19cd..c200b55 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -48,8 +48,8 @@
if (!ddend0.isApprox(ddstart1, 1e-6)) {
AOS_LOG(
ERROR,
- "Splines %d and %d don't line up in the second derivative. [%f, "
- "%f] != [%f, %f]\n",
+ "Splines %d and %d don't line up in the second derivative. [%.7f, "
+ "%.7f] != [%.7f, %.7f]\n",
static_cast<int>(i - 1), static_cast<int>(i), ddend0(0, 0),
ddend0(1, 0), ddstart1(0, 0), ddstart1(1, 0));
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 89dc3d5..8158e75 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -77,6 +77,7 @@
left_velocity:float (id: 9);
right_velocity:float (id: 10);
distance_remaining:float (id: 11);
+ distance_traveled:float (id: 13);
// Splines that we have full plans for.
available_splines:[int] (id: 12);
diff --git a/frc971/control_loops/drivetrain/localization/localizer_output.fbs b/frc971/control_loops/drivetrain/localization/localizer_output.fbs
index ff25c31..8955b96 100644
--- a/frc971/control_loops/drivetrain/localization/localizer_output.fbs
+++ b/frc971/control_loops/drivetrain/localization/localizer_output.fbs
@@ -37,6 +37,9 @@
// Cumulative number of accepted images.
image_accepted_count:uint (id: 7);
+
+ // Is true if all vision pis are connected to the IMU pi
+ all_pis_connected:bool (id: 8);
}
root_type LocalizerOutput;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0f9418c..ddcf63d 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -256,6 +256,8 @@
? CHECK_NOTNULL(current_trajectory())->length() - current_xva_.x()
: 0.0);
trajectory_logging_builder.add_available_splines(handles_vector);
+ trajectory_logging_builder.add_distance_traveled(
+ executing_spline_ ? current_xva_.x() : 0.0);
return trajectory_logging_builder.Finish();
}
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
index 1cc6f57..16c792bc 100644
--- a/frc971/control_loops/python/graph.py
+++ b/frc971/control_loops/python/graph.py
@@ -23,8 +23,10 @@
self.canvas = FigureCanvas(fig) # a Gtk.DrawingArea
self.canvas.set_vexpand(True)
self.canvas.set_size_request(800, 250)
- self.callback_id = self.canvas.mpl_connect('motion_notify_event',
- self.on_mouse_move)
+ self.mouse_move_callback = self.canvas.mpl_connect(
+ 'motion_notify_event', self.on_mouse_move)
+ self.click_callback = self.canvas.mpl_connect('button_press_event',
+ self.on_click)
self.add(self.canvas)
# The current graph data
@@ -99,6 +101,24 @@
if self.cursor_watcher is not None:
self.cursor_watcher.queue_draw()
+ def on_click(self, event):
+ """Same as on_mouse_move but also selects multisplines"""
+
+ if self.data is None:
+ return
+ total_steps_taken = self.data.shape[1]
+ total_time = self.dt * total_steps_taken
+ if event.xdata is not None:
+ # clip the position if still on the canvas, but off the graph
+ self.cursor = np.clip(event.xdata, 0, total_time)
+
+ self.redraw_cursor()
+
+ # tell the field to update too
+ if self.cursor_watcher is not None:
+ self.cursor_watcher.queue_draw()
+ self.cursor_watcher.on_graph_clicked()
+
def redraw_cursor(self):
"""Redraws the cursor line"""
# TODO: This redraws the entire graph and isn't very snappy
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index b5e8c1b..1659e55 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -287,7 +287,11 @@
if i == 0:
self.draw_robot_at_point(cr, spline, 0)
- self.draw_robot_at_point(cr, spline, 1)
+
+ is_last_spline = spline is multispline.getLibsplines()[-1]
+
+ if multispline == self.active_multispline or is_last_spline:
+ self.draw_robot_at_point(cr, spline, 1)
def export_json(self, file_name):
export_folder = Path(
@@ -422,6 +426,15 @@
prev_multispline.getSplines()[-1])
self.queue_draw()
+ def on_graph_clicked(self):
+ if self.graph.cursor is not None:
+ cursor = self.graph.find_cursor()
+ if cursor is None:
+ return
+ multispline_index, x = cursor
+
+ self.active_multispline_index = multispline_index
+
def do_button_release_event(self, event):
self.drag_start = None
@@ -489,7 +502,7 @@
multispline, result = Multispline.nearest_distance(
self.multisplines, cur_p)
- if result and result.fun < 0.1:
+ if self.control_point_index == None and result and result.fun < 0.1:
self.active_multispline_index = self.multisplines.index(
multispline)
diff --git a/frc971/imu_reader/imu_failures.fbs b/frc971/imu_reader/imu_failures.fbs
index e6d9d85..50764a2 100644
--- a/frc971/imu_reader/imu_failures.fbs
+++ b/frc971/imu_reader/imu_failures.fbs
@@ -13,4 +13,12 @@
// Total number of messages dropped for any other conditions that can fault
// the zeroer (e.g., diagnostic failures in the IMU).
other_zeroing_faults:uint (id: 3);
+ // These are the counts for the number of times that we think the pico and imu
+ // have reset. Note that when the pico resets both of these should increment.
+ probable_pico_reset_count:uint (id: 4);
+ probable_imu_reset_count:uint (id: 5);
+ // This counter is used for things that look like resets (i.e., a large gap in
+ // IMU readings) but where the IMU data counter and pico timestamp offsets
+ // still look correct.
+ unassignable_reset_count:uint (id: 6);
}
diff --git a/frc971/imu_reader/imu_watcher.cc b/frc971/imu_reader/imu_watcher.cc
index e4e100a..153b84f 100644
--- a/frc971/imu_reader/imu_watcher.cc
+++ b/frc971/imu_reader/imu_watcher.cc
@@ -29,7 +29,7 @@
right_encoder_(
-EncoderWrapDistance(drivetrain_distance_per_encoder_tick) / 2.0,
EncoderWrapDistance(drivetrain_distance_per_encoder_tick)) {
- event_loop->MakeWatcher("/localizer", [this, timestamp_source](
+ event_loop->MakeWatcher("/localizer", [this, timestamp_source, event_loop](
const IMUValuesBatch &values) {
CHECK(values.has_readings());
for (const IMUValues *value : *values.readings()) {
@@ -47,6 +47,7 @@
first_valid_data_counter_ = value->data_counter();
}
}
+ int messages_dropped_this_cycle = 0;
if (first_valid_data_counter_.has_value()) {
total_imu_messages_received_++;
// Only update when we have good checksums, since the data counter
@@ -55,10 +56,13 @@
if (value->data_counter() < last_data_counter_) {
data_counter_offset_ += 1 << 16;
}
- imu_fault_tracker_.missed_messages =
+ const int total_dropped =
(1 + value->data_counter() + data_counter_offset_ -
first_valid_data_counter_.value()) -
total_imu_messages_received_;
+ messages_dropped_this_cycle =
+ total_dropped - imu_fault_tracker_.missed_messages;
+ imu_fault_tracker_.missed_messages = total_dropped;
last_data_counter_ = value->data_counter();
}
}
@@ -92,12 +96,48 @@
pico_offset_ = pi_read_timestamp - pico_timestamp;
last_pico_timestamp_ = pico_timestamp;
}
+ // The pico will sleep for a minimum of 3 seconds when resetting the
+ // IMU. Any absence of messages for less than that time is probably not
+ // an actual reset.
if (pico_timestamp < last_pico_timestamp_) {
pico_offset_.value() += std::chrono::microseconds(1ULL << 32);
}
const aos::monotonic_clock::time_point sample_timestamp =
pico_offset_.value() + pico_timestamp;
pico_offset_error_ = pi_read_timestamp - sample_timestamp;
+
+ if (last_imu_message_.has_value()) {
+ constexpr std::chrono::seconds kMinimumImuResetTime(3);
+ const std::chrono::nanoseconds message_time_gap =
+ last_imu_message_.value() -
+ event_loop->context().monotonic_event_time;
+ if (message_time_gap > kMinimumImuResetTime) {
+ bool assigned_fault = false;
+ // The IMU has probably reset; attempt to determine whether just the
+ // IMU was reset or if the IMU and pico reset.
+ if (std::chrono::abs(pico_offset_error_) >
+ std::chrono::seconds(1)) {
+ // If we have this big of a gap, then everything downstream of
+ // this is going to complain anyways, so reset the offset.
+ pico_offset_.reset();
+
+ imu_fault_tracker_.probable_pico_reset_count++;
+ assigned_fault = true;
+ }
+ // See if we dropped the "right" number of messages for the gap in
+ // question.
+ if (std::chrono::abs(messages_dropped_this_cycle * kNominalDt -
+ message_time_gap) >
+ std::chrono::milliseconds(100)) {
+ imu_fault_tracker_.probable_imu_reset_count++;
+ assigned_fault = true;
+ }
+
+ if (!assigned_fault) {
+ imu_fault_tracker_.unassignable_reset_count++;
+ }
+ }
+ }
const bool zeroed = zeroer_.Zeroed();
// When not zeroed, we aim to approximate zero acceleration by doing a
@@ -114,6 +154,7 @@
}
last_pico_timestamp_ = pico_timestamp;
}
+ last_imu_message_ = event_loop->context().monotonic_event_time;
}
});
}
diff --git a/frc971/imu_reader/imu_watcher.h b/frc971/imu_reader/imu_watcher.h
index fe1f77d..2b32371 100644
--- a/frc971/imu_reader/imu_watcher.h
+++ b/frc971/imu_reader/imu_watcher.h
@@ -81,6 +81,8 @@
// Last observed pico measurement. Used to track IMU staleness.
std::optional<aos::monotonic_clock::time_point> last_pico_timestamp_;
+ // Time at which we received the last imu message on the pi's clock.
+ std::optional<aos::monotonic_clock::time_point> last_imu_message_;
// Estimate of the drift between the pi and pico clocks. See
// pico_offset_error() for definition.
aos::monotonic_clock::duration pico_offset_error_;
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..a25e6c3
--- /dev/null
+++ b/frc971/input/redundant_joystick_data.cc
@@ -0,0 +1,103 @@
+#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) = -1;
+ }
+
+ 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 {
+ if (joystick < 0 || joystick >= static_cast<int>(joystick_map_.size())) {
+ return -1;
+ }
+ 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/rockpi/build_rootfs.sh b/frc971/rockpi/build_rootfs.sh
index d19a260..d61fb48 100755
--- a/frc971/rockpi/build_rootfs.sh
+++ b/frc971/rockpi/build_rootfs.sh
@@ -229,6 +229,7 @@
copyfile root.root 644 etc/udev/rules.d/99-usb-mount.rules
copyfile root.root 644 etc/udev/rules.d/99-adis16505.rules
copyfile root.root 644 etc/udev/rules.d/99-mali.rules
+copyfile root.root 644 etc/udev/rules.d/99-coral-perms.rules
copyfile root.root 644 etc/chrony/chrony.conf
target "apt-get update"
diff --git a/frc971/rockpi/contents/etc/udev/rules.d/99-coral-perms.rules b/frc971/rockpi/contents/etc/udev/rules.d/99-coral-perms.rules
new file mode 100644
index 0000000..1e9deaf
--- /dev/null
+++ b/frc971/rockpi/contents/etc/udev/rules.d/99-coral-perms.rules
@@ -0,0 +1 @@
+SUBSYSTEM=="usb", MODE="0666", GROUP="plugdev"
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 133281b..84f3be7 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -166,6 +166,7 @@
":target_map_fbs",
"//aos/events:simulated_event_loop",
"//frc971/control_loops:control_loop",
+ "//frc971/vision:visualize_robot",
"//frc971/vision/ceres:pose_graph_3d_lib",
"//third_party:opencv",
"@com_google_ceres_solver//:ceres",
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index dea883b..0d6b9e9 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -10,9 +10,12 @@
DEFINE_double(distortion_noise_scalar, 1.0,
"Scale the target pose distortion factor by this when computing "
"the noise.");
-DEFINE_uint64(
+DEFINE_int32(
frozen_target_id, 1,
"Freeze the pose of this target so the map can have one fixed point.");
+DEFINE_int32(min_target_id, 1, "Minimum target id to solve for");
+DEFINE_int32(max_target_id, 8, "Maximum target id to solve for");
+DEFINE_bool(visualize_solver, false, "If true, visualize the solving process.");
namespace frc971::vision {
Eigen::Affine3d PoseUtils::Pose3dToAffine3d(
@@ -174,12 +177,14 @@
Q_vision(kOrientation3, kOrientation3) = std::pow(0.02, 2);
// Add uncertainty for the 2 vision measurements (1 at start and 1 at end)
- P += Q_vision * std::pow(detection_start.distance_from_camera, 2) *
- (1.0 +
- FLAGS_distortion_noise_scalar * detection_start.distortion_factor);
- P +=
- Q_vision * std::pow(detection_end.distance_from_camera, 2) *
- (1.0 + FLAGS_distortion_noise_scalar * detection_end.distortion_factor);
+ P += Q_vision * std::pow(detection_start.distance_from_camera *
+ (1.0 + FLAGS_distortion_noise_scalar *
+ detection_start.distortion_factor),
+ 2);
+ P += Q_vision * std::pow(detection_end.distance_from_camera *
+ (1.0 + FLAGS_distortion_noise_scalar *
+ detection_end.distortion_factor),
+ 2);
}
return P.inverse();
@@ -209,19 +214,28 @@
TargetMapper::TargetMapper(
std::string_view target_poses_path,
const ceres::examples::VectorOfConstraints &target_constraints)
- : target_constraints_(target_constraints) {
+ : target_constraints_(target_constraints),
+ T_frozen_actual_(Eigen::Vector3d::Zero()),
+ R_frozen_actual_(Eigen::Quaterniond::Identity()),
+ vis_robot_(cv::Size(1280, 1000)) {
aos::FlatbufferDetachedBuffer<TargetMap> target_map =
aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
- target_poses_[target_pose_fbs->id()] =
+ ideal_target_poses_[target_pose_fbs->id()] =
PoseUtils::TargetPoseFromFbs(*target_pose_fbs).pose;
}
+ target_poses_ = ideal_target_poses_;
}
TargetMapper::TargetMapper(
const ceres::examples::MapOfPoses &target_poses,
const ceres::examples::VectorOfConstraints &target_constraints)
- : target_poses_(target_poses), target_constraints_(target_constraints) {}
+ : ideal_target_poses_(target_poses),
+ target_poses_(ideal_target_poses_),
+ target_constraints_(target_constraints),
+ T_frozen_actual_(Eigen::Vector3d::Zero()),
+ R_frozen_actual_(Eigen::Quaterniond::Identity()),
+ vis_robot_(cv::Size(1280, 1000)) {}
std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById(
std::vector<TargetMapper::TargetPose> target_poses, TargetId target_id) {
@@ -235,9 +249,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;
@@ -246,7 +260,7 @@
// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
// Constructs the nonlinear least squares optimization problem from the pose
// graph constraints.
-void TargetMapper::BuildOptimizationProblem(
+void TargetMapper::BuildTargetPoseOptimizationProblem(
const ceres::examples::VectorOfConstraints &constraints,
ceres::examples::MapOfPoses *poses, ceres::Problem *problem) {
CHECK(poses != nullptr);
@@ -309,6 +323,33 @@
problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
}
+void TargetMapper::BuildMapFittingOptimizationProblem(ceres::Problem *problem) {
+ // Setup robot visualization
+ vis_robot_.ClearImage();
+ constexpr int kImageWidth = 1280;
+ constexpr double kFocalLength = 500.0;
+ vis_robot_.SetDefaultViewpoint(kImageWidth, kFocalLength);
+
+ const size_t num_targets = FLAGS_max_target_id - FLAGS_min_target_id;
+ // Translation and rotation error for each target
+ const size_t num_residuals = num_targets * 6;
+ // Set up the only cost function (also known as residual). This uses
+ // auto-differentiation to obtain the derivative (jacobian).
+ ceres::CostFunction *cost_function =
+ new ceres::AutoDiffCostFunction<TargetMapper, ceres::DYNAMIC, 3, 4>(
+ this, num_residuals, ceres::DO_NOT_TAKE_OWNERSHIP);
+
+ ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0);
+ ceres::LocalParameterization *quaternion_local_parameterization =
+ new ceres::EigenQuaternionParameterization;
+
+ problem->AddResidualBlock(cost_function, loss_function,
+ T_frozen_actual_.vector().data(),
+ R_frozen_actual_.coeffs().data());
+ problem->SetParameterization(R_frozen_actual_.coeffs().data(),
+ quaternion_local_parameterization);
+}
+
// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
bool TargetMapper::SolveOptimizationProblem(ceres::Problem *problem) {
CHECK_NOTNULL(problem);
@@ -328,11 +369,39 @@
void TargetMapper::Solve(std::string_view field_name,
std::optional<std::string_view> output_dir) {
- ceres::Problem problem;
- BuildOptimizationProblem(target_constraints_, &target_poses_, &problem);
+ ceres::Problem target_pose_problem;
+ BuildTargetPoseOptimizationProblem(target_constraints_, &target_poses_,
+ &target_pose_problem);
+ CHECK(SolveOptimizationProblem(&target_pose_problem))
+ << "The target pose solve was not successful, exiting.";
- CHECK(SolveOptimizationProblem(&problem))
- << "The solve was not successful, exiting.";
+ ceres::Problem map_fitting_problem;
+ BuildMapFittingOptimizationProblem(&map_fitting_problem);
+ CHECK(SolveOptimizationProblem(&map_fitting_problem))
+ << "The map fitting solve was not successful, exiting.";
+
+ Eigen::Affine3d H_frozen_actual = T_frozen_actual_ * R_frozen_actual_;
+ LOG(INFO) << "H_frozen_actual: "
+ << PoseUtils::Affine3dToPose3d(H_frozen_actual);
+
+ auto H_world_frozen =
+ PoseUtils::Pose3dToAffine3d(target_poses_[FLAGS_frozen_target_id]);
+ auto H_world_frozenactual = H_world_frozen * H_frozen_actual;
+
+ // Offset the solved poses to become the actual ones
+ for (auto &[id, pose] : target_poses_) {
+ // Don't offset targets we didn't solve for
+ if (id < FLAGS_min_target_id || id > FLAGS_max_target_id) {
+ continue;
+ }
+
+ // Take the delta between the frozen target and the solved target, and put
+ // that on top of the actual pose of the frozen target
+ auto H_world_solved = PoseUtils::Pose3dToAffine3d(pose);
+ auto H_frozen_solved = H_world_frozen.inverse() * H_world_solved;
+ auto H_world_actual = H_world_frozenactual * H_frozen_solved;
+ pose = PoseUtils::Affine3dToPose3d(H_world_actual);
+ }
auto map_json = MapToJson(field_name);
VLOG(1) << "Solved target poses: " << map_json;
@@ -364,8 +433,114 @@
{.multi_line = true});
}
+namespace {
+
+// Hacks to extract a double from a scalar, which is either a ceres jet or a
+// double. Only used for debugging and displaying.
+template <typename S>
+double ScalarToDouble(S s) {
+ const double *ptr = reinterpret_cast<double *>(&s);
+ return *ptr;
+}
+
+template <typename S>
+Eigen::Affine3d ScalarAffineToDouble(Eigen::Transform<S, 3, Eigen::Affine> H) {
+ Eigen::Affine3d H_double;
+ for (size_t i = 0; i < H.rows(); i++) {
+ for (size_t j = 0; j < H.cols(); j++) {
+ H_double(i, j) = ScalarToDouble(H(i, j));
+ }
+ }
+ return H_double;
+}
+
+} // namespace
+
+template <typename S>
+bool TargetMapper::operator()(const S *const translation,
+ const S *const rotation, S *residual) const {
+ using Affine3s = Eigen::Transform<S, 3, Eigen::Affine>;
+ Eigen::Quaternion<S> R_frozen_actual(rotation[3], rotation[1], rotation[2],
+ rotation[0]);
+ Eigen::Translation<S, 3> T_frozen_actual(translation[0], translation[1],
+ translation[2]);
+ // Actual target pose in the frame of the fixed pose.
+ Affine3s H_frozen_actual = T_frozen_actual * R_frozen_actual;
+ VLOG(2) << "H_frozen_actual: "
+ << PoseUtils::Affine3dToPose3d(ScalarAffineToDouble(H_frozen_actual));
+
+ Affine3s H_world_frozen =
+ PoseUtils::Pose3dToAffine3d(target_poses_.at(FLAGS_frozen_target_id))
+ .cast<S>();
+ Affine3s H_world_frozenactual = H_world_frozen * H_frozen_actual;
+
+ size_t residual_index = 0;
+ if (FLAGS_visualize_solver) {
+ vis_robot_.ClearImage();
+ }
+
+ for (const auto &[id, solved_pose] : target_poses_) {
+ if (id < FLAGS_min_target_id || id > FLAGS_max_target_id) {
+ continue;
+ }
+
+ Affine3s H_world_ideal =
+ PoseUtils::Pose3dToAffine3d(ideal_target_poses_.at(id)).cast<S>();
+ Affine3s H_world_solved =
+ PoseUtils::Pose3dToAffine3d(solved_pose).cast<S>();
+ // Take the delta between the frozen target and the solved target, and put
+ // that on top of the actual pose of the frozen target
+ auto H_frozen_solved = H_world_frozen.inverse() * H_world_solved;
+ auto H_world_actual = H_world_frozenactual * H_frozen_solved;
+ VLOG(2) << id << ": " << H_world_actual.translation();
+ Affine3s H_ideal_actual = H_world_ideal.inverse() * H_world_actual;
+ auto T_ideal_actual = H_ideal_actual.translation();
+ VLOG(2) << "T_ideal_actual: " << T_ideal_actual;
+ VLOG(2);
+ auto R_ideal_actual = Eigen::AngleAxis<S>(H_ideal_actual.rotation());
+
+ constexpr double kTranslationScalar = 100.0;
+ constexpr double kRotationScalar = 1000.0;
+
+ // Penalize based on how much our actual poses matches the ideal
+ // ones. We've already solved for the relative poses, now figure out
+ // where all of them fit in the world.
+ residual[residual_index++] = kTranslationScalar * T_ideal_actual(0);
+ residual[residual_index++] = kTranslationScalar * T_ideal_actual(1);
+ residual[residual_index++] = kTranslationScalar * T_ideal_actual(2);
+ residual[residual_index++] =
+ kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().x();
+ residual[residual_index++] =
+ kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().y();
+ residual[residual_index++] =
+ kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().z();
+
+ if (FLAGS_visualize_solver) {
+ vis_robot_.DrawFrameAxes(ScalarAffineToDouble(H_world_actual),
+ std::to_string(id), cv::Scalar(0, 255, 0));
+ vis_robot_.DrawFrameAxes(ScalarAffineToDouble(H_world_ideal),
+ std::to_string(id), cv::Scalar(255, 255, 255));
+ }
+ }
+ if (FLAGS_visualize_solver) {
+ cv::imshow("Target maps", vis_robot_.image_);
+ cv::waitKey(0);
+ }
+
+ // Ceres can't handle residual values of exactly zero
+ for (size_t i = 0; i < residual_index; i++) {
+ if (residual[i] == S(0)) {
+ residual[i] = S(1e-9);
+ }
+ }
+
+ return true;
+}
+
+} // namespace frc971::vision
+
std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose) {
- auto rpy = PoseUtils::QuaternionToEulerAngles(pose.q);
+ auto rpy = frc971::vision::PoseUtils::QuaternionToEulerAngles(pose.q);
os << absl::StrFormat(
"{x: %.3f, y: %.3f, z: %.3f, roll: %.3f, pitch: "
"%.3f, yaw: %.3f}",
@@ -380,5 +555,3 @@
<< constraint.t_be << "}";
return os;
}
-
-} // namespace frc971::vision
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 35c0977..ca36866 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -7,6 +7,7 @@
#include "ceres/ceres.h"
#include "frc971/vision/ceres/types.h"
#include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/visualize_robot.h"
namespace frc971::vision {
@@ -47,22 +48,40 @@
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_; }
+ // Cost function for the secondary solver finding out where the whole map fits
+ // in the world
+ template <typename S>
+ bool operator()(const S *const translation, const S *const rotation,
+ S *residual) const;
+
private:
// Constructs the nonlinear least squares optimization problem from the
// pose graph constraints.
- void BuildOptimizationProblem(
+ void BuildTargetPoseOptimizationProblem(
const ceres::examples::VectorOfConstraints &constraints,
ceres::examples::MapOfPoses *poses, ceres::Problem *problem);
+ // Constructs the nonlinear least squares optimization problem for the solved
+ // -> actual pose solver.
+ void BuildMapFittingOptimizationProblem(ceres::Problem *problem);
+
// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem *problem);
+ ceres::examples::MapOfPoses ideal_target_poses_;
ceres::examples::MapOfPoses target_poses_;
ceres::examples::VectorOfConstraints target_constraints_;
+
+ // Transformation moving the target map we solved for to where it actually
+ // should be in the world
+ Eigen::Translation3d T_frozen_actual_;
+ Eigen::Quaterniond R_frozen_actual_;
+
+ mutable VisualizeRobot vis_robot_;
};
// Utility functions for dealing with ceres::examples::Pose3d structs
@@ -136,7 +155,6 @@
const TimestampedDetection &detection_start,
const TimestampedDetection &detection_end);
- private:
// Computes the constraint between the start and end pose of the targets: the
// relative pose between the start and end target locations in the frame of
// the start target.
@@ -146,10 +164,10 @@
const TargetMapper::ConfidenceMatrix &confidence);
};
+} // namespace frc971::vision
+
std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose);
std::ostream &operator<<(std::ostream &os,
ceres::examples::Constraint3d constraint);
-} // namespace frc971::vision
-
#endif // FRC971_VISION_TARGET_MAPPER_H_
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index cd7d18b..1371f89 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -9,6 +9,9 @@
#include "glog/logging.h"
#include "gtest/gtest.h"
+DECLARE_int32(min_target_id);
+DECLARE_int32(max_target_id);
+
namespace frc971::vision {
namespace {
@@ -338,6 +341,9 @@
}
TEST(TargetMapperTest, TwoTargetsOneConstraint) {
+ FLAGS_min_target_id = 0;
+ FLAGS_max_target_id = 1;
+
ceres::examples::MapOfPoses target_poses;
target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
target_poses[1] = Make2dPose(-5.0, 0.0, 0.0);
@@ -361,6 +367,9 @@
}
TEST(TargetMapperTest, TwoTargetsTwoConstraints) {
+ FLAGS_min_target_id = 0;
+ FLAGS_max_target_id = 1;
+
ceres::examples::MapOfPoses target_poses;
target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
target_poses[1] = Make2dPose(-5.0, 0.0, -M_PI_2);
@@ -390,6 +399,9 @@
}
TEST(TargetMapperTest, TwoTargetsOneNoisyConstraint) {
+ FLAGS_min_target_id = 0;
+ FLAGS_max_target_id = 1;
+
ceres::examples::MapOfPoses target_poses;
target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
target_poses[1] = Make2dPose(-5.0, 0.0, 0.0);
@@ -549,33 +561,6 @@
.value();
EXPECT_POSE_NEAR(mapper_target_pose, actual_target_pose.pose);
}
-
- //
- // See what happens when we don't start with the
- // correct values
- //
- for (auto [target_id, target_pose] : target_poses) {
- // Skip first pose, since that needs to be correct
- // and is fixed in the solver
- if (target_id != 1) {
- ceres::examples::Pose3d bad_pose{
- Eigen::Vector3d::Zero(),
- PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d::Zero())};
- target_poses[target_id] = bad_pose;
- }
- }
-
- frc971::vision::TargetMapper mapper_bad_poses(target_poses,
- target_constraints);
- mapper_bad_poses.Solve(kFieldName);
-
- for (auto [target_pose_id, mapper_target_pose] :
- mapper_bad_poses.target_poses()) {
- TargetMapper::TargetPose actual_target_pose =
- TargetMapper::GetTargetPoseById(actual_target_poses, target_pose_id)
- .value();
- EXPECT_POSE_NEAR(mapper_target_pose, actual_target_pose.pose);
- }
}
} // namespace frc971::vision
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/scouting/db/db.go b/scouting/db/db.go
index 718711c..34f2dac 100644
--- a/scouting/db/db.go
+++ b/scouting/db/db.go
@@ -37,6 +37,7 @@
LowConesAuto, MiddleConesAuto, HighConesAuto, ConesDroppedAuto int32
LowCubes, MiddleCubes, HighCubes, CubesDropped int32
LowCones, MiddleCones, HighCones, ConesDropped int32
+ SuperchargedPieces int32
AvgCycle int64
DockedAuto, EngagedAuto, BalanceAttemptAuto bool
Docked, Engaged, BalanceAttempt bool
@@ -48,14 +49,14 @@
}
type Action struct {
- TeamNumber string `gorm:"primaryKey"`
- MatchNumber int32 `gorm:"primaryKey"`
- SetNumber int32 `gorm:"primaryKey"`
- CompLevel string `gorm:"primaryKey"`
- CompletedAction []byte
+ TeamNumber string `gorm:"primaryKey"`
+ MatchNumber int32 `gorm:"primaryKey"`
+ SetNumber int32 `gorm:"primaryKey"`
+ CompLevel string `gorm:"primaryKey"`
// This contains a serialized scouting.webserver.requests.ActionType flatbuffer.
- TimeStamp int32 `gorm:"primaryKey"`
- CollectedBy string
+ CompletedAction []byte
+ Timestamp int64 `gorm:"primaryKey"`
+ CollectedBy string
}
type NotesData struct {
@@ -64,7 +65,7 @@
Notes string
GoodDriving bool
BadDriving bool
- SolidPickup bool
+ SolidPlacing bool
SketchyPlacing bool
GoodDefense bool
BadDefense bool
@@ -308,7 +309,7 @@
Notes: data.Notes,
GoodDriving: data.GoodDriving,
BadDriving: data.BadDriving,
- SolidPickup: data.SolidPickup,
+ SolidPlacing: data.SolidPlacing,
SketchyPlacing: data.SketchyPlacing,
GoodDefense: data.GoodDefense,
BadDefense: data.BadDefense,
diff --git a/scouting/db/db_test.go b/scouting/db/db_test.go
index ea83fa3..9c48cba 100644
--- a/scouting/db/db_test.go
+++ b/scouting/db/db_test.go
@@ -150,7 +150,7 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 2,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 2,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 0, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "emma",
@@ -162,7 +162,7 @@
LowConesAuto: 2, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 0,
HighCubes: 0, CubesDropped: 1, LowCones: 0,
- MiddleCones: 0, HighCones: 1, ConesDropped: 0,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 1,
AvgCycle: 0, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: true, Engaged: true,
BalanceAttempt: false, CollectedBy: "tyler",
@@ -174,7 +174,7 @@
LowConesAuto: 0, MiddleConesAuto: 2, HighConesAuto: 1,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 0,
HighCubes: 2, CubesDropped: 1, LowCones: 1,
- MiddleCones: 1, HighCones: 0, ConesDropped: 1,
+ MiddleCones: 1, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 0, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "isaac",
@@ -186,7 +186,7 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 0, LowCubes: 0, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 0,
- MiddleCones: 1, HighCones: 0, ConesDropped: 0,
+ MiddleCones: 1, HighCones: 0, ConesDropped: 0, SuperchargedPieces: 0,
AvgCycle: 0, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "will",
@@ -198,7 +198,7 @@
LowConesAuto: 0, MiddleConesAuto: 1, HighConesAuto: 1,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 0,
HighCubes: 0, CubesDropped: 2, LowCones: 1,
- MiddleCones: 1, HighCones: 0, ConesDropped: 1,
+ MiddleCones: 1, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 0, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: true, Engaged: false,
BalanceAttempt: false, CollectedBy: "unkown",
@@ -248,7 +248,7 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 2,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 2,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 0, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "emma",
@@ -260,7 +260,7 @@
LowConesAuto: 2, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 0,
HighCubes: 0, CubesDropped: 1, LowCones: 0,
- MiddleCones: 0, HighCones: 1, ConesDropped: 0,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 1,
AvgCycle: 0, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "tyler",
@@ -272,7 +272,7 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 2,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 2,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 0, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: true, Engaged: false,
BalanceAttempt: true, CollectedBy: "emma",
@@ -329,7 +329,7 @@
LowConesAuto: 0, MiddleConesAuto: 2, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 1,
- MiddleCones: 0, HighCones: 1, ConesDropped: 2,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 2, SuperchargedPieces: 0,
AvgCycle: 58, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: true, Engaged: true,
BalanceAttempt: false, CollectedBy: "unknown",
@@ -341,7 +341,7 @@
LowConesAuto: 0, MiddleConesAuto: 1, HighConesAuto: 0,
ConesDroppedAuto: 0, LowCubes: 2, MiddleCubes: 0,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 0,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 1,
AvgCycle: 34, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: true, Engaged: false,
BalanceAttempt: false, CollectedBy: "simon",
@@ -353,7 +353,7 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 2,
- MiddleCones: 0, HighCones: 1, ConesDropped: 1,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 50, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "eliza",
@@ -365,7 +365,7 @@
LowConesAuto: 0, MiddleConesAuto: 2, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 49, DockedAuto: true, EngagedAuto: false,
Docked: false, Engaged: false, CollectedBy: "isaac",
},
@@ -376,7 +376,7 @@
LowConesAuto: 1, MiddleConesAuto: 1, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 1,
- MiddleCones: 1, HighCones: 1, ConesDropped: 0,
+ MiddleCones: 1, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 0,
AvgCycle: 70, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "sam",
@@ -391,7 +391,7 @@
LowConesAuto: 1, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 2,
- MiddleCones: 0, HighCones: 1, ConesDropped: 1,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 50, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "eliza",
@@ -403,7 +403,7 @@
LowConesAuto: 1, MiddleConesAuto: 1, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 1,
- MiddleCones: 1, HighCones: 1, ConesDropped: 0,
+ MiddleCones: 1, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 0,
AvgCycle: 70, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "sam",
@@ -683,7 +683,7 @@
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 2,
ConesDroppedAuto: 1, LowCubes: 1, MiddleCubes: 2,
HighCubes: 1, CubesDropped: 0, LowCones: 2,
- MiddleCones: 0, HighCones: 2, ConesDropped: 1,
+ MiddleCones: 0, HighCones: 2, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 51, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "isaac",
@@ -695,7 +695,7 @@
LowConesAuto: 1, MiddleConesAuto: 1, HighConesAuto: 0,
ConesDroppedAuto: 0, LowCubes: 2, MiddleCubes: 2,
HighCubes: 1, CubesDropped: 0, LowCones: 1,
- MiddleCones: 0, HighCones: 2, ConesDropped: 1,
+ MiddleCones: 0, HighCones: 2, ConesDropped: 1, SuperchargedPieces: 1,
AvgCycle: 39, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "jack",
@@ -707,7 +707,7 @@
LowConesAuto: 2, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 2, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 1,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 45, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "martin",
@@ -719,7 +719,7 @@
LowConesAuto: 2, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 2, MiddleCubes: 2,
HighCubes: 0, CubesDropped: 0, LowCones: 2,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 34, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: true, Engaged: false,
BalanceAttempt: false, CollectedBy: "unknown",
@@ -761,27 +761,27 @@
correct := []Action{
Action{
TeamNumber: "1235", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
- CompletedAction: []byte(""), TimeStamp: 0000, CollectedBy: "",
+ CompletedAction: []byte(""), Timestamp: 0000, CollectedBy: "",
},
Action{
TeamNumber: "1236", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
- CompletedAction: []byte(""), TimeStamp: 0321, CollectedBy: "",
+ CompletedAction: []byte(""), Timestamp: 0321, CollectedBy: "",
},
Action{
TeamNumber: "1237", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
- CompletedAction: []byte(""), TimeStamp: 0222, CollectedBy: "",
+ CompletedAction: []byte(""), Timestamp: 0222, CollectedBy: "",
},
Action{
TeamNumber: "1238", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
- CompletedAction: []byte(""), TimeStamp: 0110, CollectedBy: "",
+ CompletedAction: []byte(""), Timestamp: 0110, CollectedBy: "",
},
Action{
TeamNumber: "1239", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
- CompletedAction: []byte(""), TimeStamp: 0004, CollectedBy: "",
+ CompletedAction: []byte(""), Timestamp: 0004, CollectedBy: "",
},
Action{
TeamNumber: "1233", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
- CompletedAction: []byte(""), TimeStamp: 0004, CollectedBy: "",
+ CompletedAction: []byte(""), Timestamp: 0005, CollectedBy: "",
},
}
@@ -875,11 +875,11 @@
expected := []string{"Note 1", "Note 3"}
- err := fixture.db.AddNotes(NotesData{TeamNumber: 1234, Notes: "Note 1", GoodDriving: true, BadDriving: false, SolidPickup: false, SketchyPlacing: true, GoodDefense: false, BadDefense: true, EasilyDefended: true})
+ err := fixture.db.AddNotes(NotesData{TeamNumber: 1234, Notes: "Note 1", GoodDriving: true, BadDriving: false, SolidPlacing: false, SketchyPlacing: true, GoodDefense: false, BadDefense: true, EasilyDefended: true})
check(t, err, "Failed to add Note")
- err = fixture.db.AddNotes(NotesData{TeamNumber: 1235, Notes: "Note 2", GoodDriving: false, BadDriving: true, SolidPickup: false, SketchyPlacing: true, GoodDefense: false, BadDefense: false, EasilyDefended: false})
+ err = fixture.db.AddNotes(NotesData{TeamNumber: 1235, Notes: "Note 2", GoodDriving: false, BadDriving: true, SolidPlacing: false, SketchyPlacing: true, GoodDefense: false, BadDefense: false, EasilyDefended: false})
check(t, err, "Failed to add Note")
- err = fixture.db.AddNotes(NotesData{TeamNumber: 1234, Notes: "Note 3", GoodDriving: true, BadDriving: false, SolidPickup: false, SketchyPlacing: true, GoodDefense: true, BadDefense: false, EasilyDefended: true})
+ err = fixture.db.AddNotes(NotesData{TeamNumber: 1234, Notes: "Note 3", GoodDriving: true, BadDriving: false, SolidPlacing: false, SketchyPlacing: true, GoodDefense: true, BadDefense: false, EasilyDefended: true})
check(t, err, "Failed to add Note")
actual, err := fixture.db.QueryNotes(1234)
diff --git a/scouting/scouting_test.cy.js b/scouting/scouting_test.cy.js
index 0237c4b..1d7620a 100644
--- a/scouting/scouting_test.cy.js
+++ b/scouting/scouting_test.cy.js
@@ -20,6 +20,17 @@
cy.get(fieldSelector).type('{selectAll}' + value);
}
+// Click on a random team in the Match list. The exact details here are not
+// important, but we need to know what they are. This could as well be any
+// other team from any other match.
+function clickSemiFinal2Match3Team5254() {
+ // On the 87th row of matches (index 86) click on the second team
+ // (index 1) which resolves to team 5254 in semi final 2 match 3.
+ cy.get('button.match-item')
+ .eq(86 * 6 + 1)
+ .click();
+}
+
// Moves the nth slider left or right. A positive "adjustBy" value moves the
// slider to the right. A negative value moves the slider to the left.
//
@@ -88,29 +99,26 @@
cy.get('.badge').eq(89).contains('Final 1 Match 3');
});
- it('should: prefill the match information.', () => {
- headerShouldBe('Matches');
+ it('should: be let users enter match information manually.', () => {
+ switchToTab('Entry');
+ headerShouldBe(' Team Selection ');
- // On the 87th row of matches (index 86) click on the second team
- // (index 1) which resolves to team 5254 in semi final 2 match 3.
- cy.get('button.match-item')
- .eq(86 * 6 + 1)
- .click();
+ setInputTo('#match_number', '3');
+ setInputTo('#team_number', '5254');
+ setInputTo('#set_number', '2');
+ setInputTo('#comp_level', '3: sf');
- headerShouldBe('Team Selection');
- cy.get('#match_number').should('have.value', '3');
- cy.get('#team_number').should('have.value', '5254');
- cy.get('#set_number').should('have.value', '2');
- cy.get('#comp_level').should('have.value', '3: sf');
+ clickButton('Next');
+
+ headerShouldBe('5254 Init ');
});
//TODO(FILIP): Verify last action when the last action header gets added.
- it('should: be able to get to submit screen in data scouting.', () => {
- switchToTab('Data Entry');
- headerShouldBe('Team Selection');
- clickButton('Next');
+ it('should: be able to submit data scouting.', () => {
+ clickSemiFinal2Match3Team5254();
// Select Starting Position.
+ headerShouldBe('5254 Init ');
cy.get('[type="radio"]').first().check();
clickButton('Start Match');
@@ -131,17 +139,15 @@
clickButton('Endgame');
cy.get('[type="checkbox"]').check();
- // Should be on submit screen.
- // TODO(FILIP): Verify that submitting works once we add it.
-
clickButton('End Match');
- headerShouldBe('Review and Submit');
+ headerShouldBe('5254 Review and Submit ');
+
+ clickButton('Submit');
+ headerShouldBe('5254 Success ');
});
it('should: be able to return to correct screen with undo for pick and place.', () => {
- switchToTab('Data Entry');
- headerShouldBe('Team Selection');
- clickButton('Next');
+ clickSemiFinal2Match3Team5254();
// Select Starting Position.
cy.get('[type="radio"]').first().check();
@@ -154,13 +160,13 @@
clickButton('UNDO');
// User should be back on pickup screen.
- headerShouldBe('Pickup');
+ headerShouldBe('5254 Pickup ');
// Check the same thing but for undoing place.
clickButton('CUBE');
clickButton('MID');
clickButton('UNDO');
- headerShouldBe('Place');
+ headerShouldBe('5254 Place ');
});
it('should: submit note scouting for multiple teams', () => {
diff --git a/scouting/webserver/requests/debug/BUILD b/scouting/webserver/requests/debug/BUILD
index f3f4a72..d9bb030 100644
--- a/scouting/webserver/requests/debug/BUILD
+++ b/scouting/webserver/requests/debug/BUILD
@@ -14,6 +14,7 @@
"//scouting/webserver/requests/messages:request_all_notes_response_go_fbs",
"//scouting/webserver/requests/messages:request_notes_for_team_response_go_fbs",
"//scouting/webserver/requests/messages:request_shift_schedule_response_go_fbs",
+ "//scouting/webserver/requests/messages:submit_actions_response_go_fbs",
"//scouting/webserver/requests/messages:submit_driver_ranking_response_go_fbs",
"//scouting/webserver/requests/messages:submit_notes_response_go_fbs",
"//scouting/webserver/requests/messages:submit_shift_schedule_response_go_fbs",
diff --git a/scouting/webserver/requests/debug/cli/cli_test.py b/scouting/webserver/requests/debug/cli/cli_test.py
index 0c2beab..b944564 100644
--- a/scouting/webserver/requests/debug/cli/cli_test.py
+++ b/scouting/webserver/requests/debug/cli/cli_test.py
@@ -105,7 +105,7 @@
"notes": "A very inspiring and useful comment",
"good_driving": True,
"bad_driving": False,
- "solid_pickup": False,
+ "solid_placing": False,
"sketchy_placing": True,
"good_defense": False,
"bad_defense": False,
@@ -128,7 +128,7 @@
Notes: (string) (len=35) "A very inspiring and useful comment",
GoodDriving: (bool) true,
BadDriving: (bool) false,
- SolidPickup: (bool) false,
+ SolidPlacing: (bool) false,
SketchyPlacing: (bool) true,
GoodDefense: (bool) false,
BadDefense: (bool) false,
diff --git a/scouting/webserver/requests/debug/debug.go b/scouting/webserver/requests/debug/debug.go
index eb3a1ca..acb9dd4 100644
--- a/scouting/webserver/requests/debug/debug.go
+++ b/scouting/webserver/requests/debug/debug.go
@@ -16,6 +16,7 @@
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_notes_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_shift_schedule_response"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_actions_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_driver_ranking_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_notes_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_shift_schedule_response"
@@ -157,3 +158,9 @@
server+"/requests/submit/submit_driver_ranking", requestBytes,
submit_driver_ranking_response.GetRootAsSubmitDriverRankingResponse)
}
+
+func SubmitActions(server string, requestBytes []byte) (*submit_actions_response.SubmitActionsResponseT, error) {
+ return sendMessage[submit_actions_response.SubmitActionsResponseT](
+ server+"/requests/submit/submit_actions", requestBytes,
+ submit_actions_response.GetRootAsSubmitActionsResponse)
+}
diff --git a/scouting/webserver/requests/messages/request_2023_data_scouting_response.fbs b/scouting/webserver/requests/messages/request_2023_data_scouting_response.fbs
index b472c58..341b012 100644
--- a/scouting/webserver/requests/messages/request_2023_data_scouting_response.fbs
+++ b/scouting/webserver/requests/messages/request_2023_data_scouting_response.fbs
@@ -24,6 +24,7 @@
middle_cones:int (id:16);
high_cones:int (id:17);
cones_dropped:int (id:18);
+ supercharged_pieces:int (id:29);
// Time in nanoseconds.
avg_cycle: int64 (id:19);
docked_auto: bool (id:20);
@@ -40,4 +41,4 @@
stats_list:[Stats2023] (id:0);
}
-root_type Request2023DataScoutingResponse;
+root_type Request2023DataScoutingResponse;
\ No newline at end of file
diff --git a/scouting/webserver/requests/messages/request_all_notes_response.fbs b/scouting/webserver/requests/messages/request_all_notes_response.fbs
index 983ff62..70504d3 100644
--- a/scouting/webserver/requests/messages/request_all_notes_response.fbs
+++ b/scouting/webserver/requests/messages/request_all_notes_response.fbs
@@ -5,7 +5,7 @@
notes:string (id: 1);
good_driving:bool (id: 2);
bad_driving:bool (id: 3);
- solid_pickup:bool (id: 4);
+ solid_placing:bool (id: 4);
sketchy_placing:bool (id: 5);
good_defense:bool (id: 6);
bad_defense:bool (id: 7);
diff --git a/scouting/webserver/requests/messages/submit_actions.fbs b/scouting/webserver/requests/messages/submit_actions.fbs
index 8c79097..d269788 100644
--- a/scouting/webserver/requests/messages/submit_actions.fbs
+++ b/scouting/webserver/requests/messages/submit_actions.fbs
@@ -12,7 +12,8 @@
enum ScoreLevel: short {
kLow,
kMiddle,
- kHigh
+ kHigh,
+ kSupercharged,
}
table AutoBalanceAction {
@@ -62,5 +63,6 @@
set_number:int (id: 2);
comp_level:string (id: 3);
actions_list:[Action] (id:4);
+ //TODO: delete this field
collected_by:string (id: 5);
}
diff --git a/scouting/webserver/requests/messages/submit_notes.fbs b/scouting/webserver/requests/messages/submit_notes.fbs
index cee4dee..845a601 100644
--- a/scouting/webserver/requests/messages/submit_notes.fbs
+++ b/scouting/webserver/requests/messages/submit_notes.fbs
@@ -5,7 +5,7 @@
notes:string (id: 1);
good_driving:bool (id: 2);
bad_driving:bool (id: 3);
- solid_pickup:bool (id: 4);
+ solid_placing:bool (id: 4);
sketchy_placing:bool (id: 5);
good_defense:bool (id: 6);
bad_defense:bool (id: 7);
diff --git a/scouting/webserver/requests/requests.go b/scouting/webserver/requests/requests.go
index 9305274..a6ee44c 100644
--- a/scouting/webserver/requests/requests.go
+++ b/scouting/webserver/requests/requests.go
@@ -56,6 +56,7 @@
type SubmitDriverRanking = submit_driver_ranking.SubmitDriverRanking
type SubmitDriverRankingResponseT = submit_driver_ranking_response.SubmitDriverRankingResponseT
type SubmitActions = submit_actions.SubmitActions
+type Action = submit_actions.Action
type SubmitActionsResponseT = submit_actions_response.SubmitActionsResponseT
// The interface we expect the database abstraction to conform to.
@@ -74,6 +75,7 @@
QueryNotes(int32) ([]string, error)
AddNotes(db.NotesData) error
AddDriverRanking(db.DriverRankingData) error
+ AddAction(db.Action) error
}
// Handles unknown requests. Just returns a 404.
@@ -353,7 +355,7 @@
Notes: string(request.Notes()),
GoodDriving: bool(request.GoodDriving()),
BadDriving: bool(request.BadDriving()),
- SolidPickup: bool(request.SolidPickup()),
+ SolidPlacing: bool(request.SolidPlacing()),
SketchyPlacing: bool(request.SketchyPlacing()),
GoodDefense: bool(request.GoodDefense()),
BadDefense: bool(request.BadDefense()),
@@ -378,7 +380,7 @@
stat := db.Stats2023{TeamNumber: string(submitActions.TeamNumber()), MatchNumber: submitActions.MatchNumber(), SetNumber: submitActions.SetNumber(), CompLevel: string(submitActions.CompLevel()),
StartingQuadrant: 0, LowCubesAuto: 0, MiddleCubesAuto: 0, HighCubesAuto: 0, CubesDroppedAuto: 0,
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0, ConesDroppedAuto: 0, LowCubes: 0, MiddleCubes: 0, HighCubes: 0,
- CubesDropped: 0, LowCones: 0, MiddleCones: 0, HighCones: 0, ConesDropped: 0, AvgCycle: 0, CollectedBy: string(submitActions.CollectedBy()),
+ CubesDropped: 0, LowCones: 0, MiddleCones: 0, HighCones: 0, ConesDropped: 0, SuperchargedPieces: 0, AvgCycle: 0, CollectedBy: string(submitActions.CollectedBy()),
}
// Loop over all actions.
for i := 0; i < submitActions.ActionsListLength(); i++ {
@@ -458,6 +460,8 @@
stat.HighConesAuto += 1
} else if object == 1 && level == 2 && auto == false {
stat.HighCones += 1
+ } else if level == 3 {
+ stat.SuperchargedPieces += 1
} else {
return db.Stats2023{}, errors.New(fmt.Sprintf("Got unknown ObjectType/ScoreLevel/Auto combination"))
}
@@ -539,6 +543,7 @@
MiddleCones: stat.MiddleCones,
HighCones: stat.HighCones,
ConesDropped: stat.ConesDropped,
+ SuperchargedPieces: stat.SuperchargedPieces,
AvgCycle: stat.AvgCycle,
DockedAuto: stat.DockedAuto,
EngagedAuto: stat.EngagedAuto,
@@ -735,7 +740,7 @@
Notes: note.Notes,
GoodDriving: note.GoodDriving,
BadDriving: note.BadDriving,
- SolidPickup: note.SolidPickup,
+ SolidPlacing: note.SolidPlacing,
SketchyPlacing: note.SketchyPlacing,
GoodDefense: note.GoodDefense,
BadDefense: note.BadDefense,
@@ -785,6 +790,62 @@
w.Write(builder.FinishedBytes())
}
+type submitActionsHandler struct {
+ db Database
+}
+
+func (handler submitActionsHandler) ServeHTTP(w http.ResponseWriter, req *http.Request) {
+ // Get the username of the person submitting the data.
+ username := parseUsername(req)
+
+ requestBytes, err := io.ReadAll(req.Body)
+ if err != nil {
+ respondWithError(w, http.StatusBadRequest, fmt.Sprint("Failed to read request bytes:", err))
+ return
+ }
+
+ request, success := parseRequest(w, requestBytes, "SubmitActions", submit_actions.GetRootAsSubmitActions)
+ if !success {
+ return
+ }
+
+ log.Println("Got actions for match", request.MatchNumber(), "team", request.TeamNumber(), "from", username)
+
+ for i := 0; i < request.ActionsListLength(); i++ {
+
+ var action Action
+ request.ActionsList(&action, i)
+
+ dbAction := db.Action{
+ TeamNumber: string(request.TeamNumber()),
+ MatchNumber: request.MatchNumber(),
+ SetNumber: request.SetNumber(),
+ CompLevel: string(request.CompLevel()),
+ //TODO: Serialize CompletedAction
+ CompletedAction: []byte{},
+ Timestamp: action.Timestamp(),
+ CollectedBy: username,
+ }
+
+ // Do some error checking.
+ if action.Timestamp() < 0 {
+ respondWithError(w, http.StatusBadRequest, fmt.Sprint(
+ "Invalid timestamp field value of ", action.Timestamp()))
+ return
+ }
+
+ err = handler.db.AddAction(dbAction)
+ if err != nil {
+ respondWithError(w, http.StatusInternalServerError, fmt.Sprint("Failed to add action to database: ", err))
+ return
+ }
+ }
+
+ builder := flatbuffers.NewBuilder(50 * 1024)
+ builder.Finish((&SubmitActionsResponseT{}).Pack(builder))
+ w.Write(builder.FinishedBytes())
+}
+
func HandleRequests(db Database, scoutingServer server.ScoutingServer) {
scoutingServer.HandleFunc("/requests", unknown)
scoutingServer.Handle("/requests/request/all_matches", requestAllMatchesHandler{db})
@@ -796,4 +857,5 @@
scoutingServer.Handle("/requests/submit/shift_schedule", submitShiftScheduleHandler{db})
scoutingServer.Handle("/requests/request/shift_schedule", requestShiftScheduleHandler{db})
scoutingServer.Handle("/requests/submit/submit_driver_ranking", SubmitDriverRankingHandler{db})
+ scoutingServer.Handle("/requests/submit/submit_actions", submitActionsHandler{db})
}
diff --git a/scouting/webserver/requests/requests_test.go b/scouting/webserver/requests/requests_test.go
index dab3174..b6f09fc 100644
--- a/scouting/webserver/requests/requests_test.go
+++ b/scouting/webserver/requests/requests_test.go
@@ -129,7 +129,7 @@
LowConesAuto: 1, MiddleConesAuto: 2, HighConesAuto: 1,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 1,
- MiddleCones: 2, HighCones: 0, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 34, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: false, CollectedBy: "alex",
@@ -141,7 +141,7 @@
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 0,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 53, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "bob",
@@ -214,7 +214,7 @@
LowConesAuto: 1, MiddleConesAuto: 2, HighConesAuto: 1,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 1,
- MiddleCones: 2, HighCones: 0, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 34, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "isaac",
@@ -226,7 +226,7 @@
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 0,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 53, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "unknown",
@@ -255,7 +255,7 @@
LowConesAuto: 1, MiddleConesAuto: 2, HighConesAuto: 1,
ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 1,
HighCubes: 2, CubesDropped: 1, LowCones: 1,
- MiddleCones: 2, HighCones: 0, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 34, DockedAuto: true, EngagedAuto: false,
BalanceAttemptAuto: false, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "isaac",
@@ -267,7 +267,7 @@
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 0,
HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1,
+ MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
AvgCycle: 53, DockedAuto: false, EngagedAuto: false,
BalanceAttemptAuto: true, Docked: false, Engaged: false,
BalanceAttempt: true, CollectedBy: "unknown",
@@ -368,6 +368,27 @@
},
{
ActionTaken: &submit_actions.ActionTypeT{
+ Type: submit_actions.ActionTypePickupObjectAction,
+ Value: &submit_actions.PickupObjectActionT{
+ ObjectType: submit_actions.ObjectTypekCube,
+ Auto: false,
+ },
+ },
+ Timestamp: 3500,
+ },
+ {
+ ActionTaken: &submit_actions.ActionTypeT{
+ Type: submit_actions.ActionTypePlaceObjectAction,
+ Value: &submit_actions.PlaceObjectActionT{
+ ObjectType: submit_actions.ObjectTypekCube,
+ ScoreLevel: submit_actions.ScoreLevelkSupercharged,
+ Auto: false,
+ },
+ },
+ Timestamp: 3900,
+ },
+ {
+ ActionTaken: &submit_actions.ActionTypeT{
Type: submit_actions.ActionTypeEndMatchAction,
Value: &submit_actions.EndMatchActionT{
Docked: true,
@@ -375,7 +396,7 @@
BalanceAttempt: true,
},
},
- Timestamp: 4000,
+ Timestamp: 4200,
},
},
}).Pack(builder))
@@ -394,8 +415,8 @@
LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0,
ConesDroppedAuto: 0, LowCubes: 0, MiddleCubes: 0,
HighCubes: 0, CubesDropped: 0, LowCones: 0,
- MiddleCones: 0, HighCones: 1, ConesDropped: 0,
- AvgCycle: 1100, DockedAuto: true, EngagedAuto: true,
+ MiddleCones: 0, HighCones: 1, ConesDropped: 0, SuperchargedPieces: 1,
+ AvgCycle: 950, DockedAuto: true, EngagedAuto: true,
BalanceAttemptAuto: false, Docked: true, Engaged: false,
BalanceAttempt: true, CollectedBy: "katie",
}
@@ -418,7 +439,7 @@
Notes: "Notes",
GoodDriving: true,
BadDriving: false,
- SolidPickup: true,
+ SolidPlacing: true,
SketchyPlacing: false,
GoodDefense: true,
BadDefense: false,
@@ -436,7 +457,7 @@
Notes: "Notes",
GoodDriving: true,
BadDriving: false,
- SolidPickup: true,
+ SolidPlacing: true,
SketchyPlacing: false,
GoodDefense: true,
BadDefense: false,
@@ -456,7 +477,7 @@
Notes: "Notes",
GoodDriving: true,
BadDriving: false,
- SolidPickup: true,
+ SolidPlacing: true,
SketchyPlacing: false,
GoodDefense: true,
BadDefense: false,
@@ -684,7 +705,7 @@
Notes: "Notes",
GoodDriving: true,
BadDriving: false,
- SolidPickup: true,
+ SolidPlacing: true,
SketchyPlacing: false,
GoodDefense: true,
BadDefense: false,
@@ -695,7 +716,7 @@
Notes: "More Notes",
GoodDriving: false,
BadDriving: false,
- SolidPickup: false,
+ SolidPlacing: false,
SketchyPlacing: true,
GoodDefense: false,
BadDefense: true,
@@ -723,7 +744,7 @@
Notes: "Notes",
GoodDriving: true,
BadDriving: false,
- SolidPickup: true,
+ SolidPlacing: true,
SketchyPlacing: false,
GoodDefense: true,
BadDefense: false,
@@ -734,7 +755,7 @@
Notes: "More Notes",
GoodDriving: false,
BadDriving: false,
- SolidPickup: false,
+ SolidPlacing: false,
SketchyPlacing: true,
GoodDefense: false,
BadDefense: true,
@@ -752,6 +773,109 @@
}
}
+func packAction(action *submit_actions.ActionT) []byte {
+ builder := flatbuffers.NewBuilder(50 * 1024)
+ builder.Finish((action).Pack(builder))
+ return (builder.FinishedBytes())
+}
+
+func TestAddingActions(t *testing.T) {
+ database := MockDatabase{}
+ scoutingServer := server.NewScoutingServer()
+ HandleRequests(&database, scoutingServer)
+ scoutingServer.Start(8080)
+ defer scoutingServer.Stop()
+
+ builder := flatbuffers.NewBuilder(1024)
+ builder.Finish((&submit_actions.SubmitActionsT{
+ TeamNumber: "1234",
+ MatchNumber: 4,
+ SetNumber: 1,
+ CompLevel: "qual",
+ ActionsList: []*submit_actions.ActionT{
+ {
+ ActionTaken: &submit_actions.ActionTypeT{
+ Type: submit_actions.ActionTypePickupObjectAction,
+ Value: &submit_actions.PickupObjectActionT{
+ ObjectType: submit_actions.ObjectTypekCube,
+ Auto: true,
+ },
+ },
+ Timestamp: 2400,
+ },
+ {
+ ActionTaken: &submit_actions.ActionTypeT{
+ Type: submit_actions.ActionTypePlaceObjectAction,
+ Value: &submit_actions.PlaceObjectActionT{
+ ObjectType: submit_actions.ObjectTypekCube,
+ ScoreLevel: submit_actions.ScoreLevelkLow,
+ Auto: false,
+ },
+ },
+ Timestamp: 1009,
+ },
+ },
+ }).Pack(builder))
+
+ _, err := debug.SubmitActions("http://localhost:8080", builder.FinishedBytes())
+ if err != nil {
+ t.Fatal("Failed to submit actions: ", err)
+ }
+
+ // Make sure that the data made it into the database.
+ // TODO: Add this back when we figure out how to add the serialized action into the database.
+
+ /* expectedActionsT := []*submit_actions.ActionT{
+ {
+ ActionTaken: &submit_actions.ActionTypeT{
+ Type: submit_actions.ActionTypePickupObjectAction,
+ Value: &submit_actions.PickupObjectActionT{
+ ObjectType: submit_actions.ObjectTypekCube,
+ Auto: true,
+ },
+ },
+ Timestamp: 2400,
+ },
+ {
+ ActionTaken: &submit_actions.ActionTypeT{
+ Type: submit_actions.ActionTypePlaceObjectAction,
+ Value: &submit_actions.PlaceObjectActionT{
+ ObjectType: submit_actions.ObjectTypekCube,
+ ScoreLevel: submit_actions.ScoreLevelkLow,
+ Auto: false,
+ },
+ },
+ Timestamp: 1009,
+ },
+ } */
+
+ expectedActions := []db.Action{
+ {
+ TeamNumber: "1234",
+ MatchNumber: 4,
+ SetNumber: 1,
+ CompLevel: "qual",
+ CollectedBy: "debug_cli",
+ CompletedAction: []byte{},
+ Timestamp: 2400,
+ },
+ {
+ TeamNumber: "1234",
+ MatchNumber: 4,
+ SetNumber: 1,
+ CompLevel: "qual",
+ CollectedBy: "debug_cli",
+ CompletedAction: []byte{},
+ Timestamp: 1009,
+ },
+ }
+
+ if !reflect.DeepEqual(expectedActions, database.actions) {
+ t.Fatal("Expected ", expectedActions, ", but got:", database.actions)
+ }
+
+}
+
// A mocked database we can use for testing. Add functionality to this as
// needed for your tests.
@@ -761,6 +885,7 @@
shiftSchedule []db.Shift
driver_ranking []db.DriverRankingData
stats2023 []db.Stats2023
+ actions []db.Action
}
func (database *MockDatabase) AddToMatch(match db.TeamMatch) error {
@@ -830,3 +955,12 @@
func (database *MockDatabase) ReturnAllDriverRankings() ([]db.DriverRankingData, error) {
return database.driver_ranking, nil
}
+
+func (database *MockDatabase) AddAction(action db.Action) error {
+ database.actions = append(database.actions, action)
+ return nil
+}
+
+func (database *MockDatabase) ReturnActions() ([]db.Action, error) {
+ return database.actions, nil
+}
diff --git a/scouting/www/app/app.ng.html b/scouting/www/app/app.ng.html
index 526cd61..b7f1873 100644
--- a/scouting/www/app/app.ng.html
+++ b/scouting/www/app/app.ng.html
@@ -72,18 +72,15 @@
*ngSwitchCase="'MatchList'"
></app-match-list>
<app-entry
- (switchTabsEvent)="switchTabTo($event)"
[teamNumber]="selectedTeamInMatch.teamNumber"
[matchNumber]="selectedTeamInMatch.matchNumber"
[setNumber]="selectedTeamInMatch.setNumber"
[compLevel]="selectedTeamInMatch.compLevel"
+ [skipTeamSelection]="navigatedFromMatchList"
*ngSwitchCase="'Entry'"
></app-entry>
<frc971-notes *ngSwitchCase="'Notes'"></frc971-notes>
<app-driver-ranking *ngSwitchCase="'DriverRanking'"></app-driver-ranking>
<shift-schedule *ngSwitchCase="'ShiftSchedule'"></shift-schedule>
- <app-view
- (switchTabsEvent)="switchTabTo($event)"
- *ngSwitchCase="'View'"
- ></app-view>
+ <app-view *ngSwitchCase="'View'"></app-view>
</ng-container>
diff --git a/scouting/www/app/app.ts b/scouting/www/app/app.ts
index f7d2770..c19895a 100644
--- a/scouting/www/app/app.ts
+++ b/scouting/www/app/app.ts
@@ -30,6 +30,9 @@
setNumber: 1,
compLevel: 'qm',
};
+ // Keep track of the match list automatically navigating the user to the
+ // Entry tab.
+ navigatedFromMatchList: boolean = false;
tab: Tab = 'MatchList';
@ViewChild('block_alerts') block_alerts: ElementRef;
@@ -54,7 +57,8 @@
selectTeamInMatch(teamInMatch: TeamInMatch) {
this.selectedTeamInMatch = teamInMatch;
- this.switchTabTo('Entry');
+ this.navigatedFromMatchList = true;
+ this.switchTabTo('Entry', false);
}
switchTabToGuarded(tab: Tab) {
@@ -69,11 +73,16 @@
}
}
if (shouldSwitch) {
- this.switchTabTo(tab);
+ this.switchTabTo(tab, true);
}
}
- private switchTabTo(tab: Tab) {
+ private switchTabTo(tab: Tab, wasGuarded: boolean) {
+ if (wasGuarded) {
+ // When the user navigated between tabs manually, we want to reset some
+ // state.
+ this.navigatedFromMatchList = false;
+ }
this.tab = tab;
}
}
diff --git a/scouting/www/entry/entry.component.ts b/scouting/www/entry/entry.component.ts
index 9ab5b7a..b56948a 100644
--- a/scouting/www/entry/entry.component.ts
+++ b/scouting/www/entry/entry.component.ts
@@ -97,7 +97,7 @@
templateUrl: './entry.ng.html',
styleUrls: ['../app/common.css', './entry.component.css'],
})
-export class EntryComponent {
+export class EntryComponent implements OnInit {
// Re-export the type here so that we can use it in the `[value]` attribute
// of radio buttons.
readonly COMP_LEVELS = COMP_LEVELS;
@@ -106,11 +106,11 @@
readonly ScoreLevel = ScoreLevel;
section: Section = 'Team Selection';
- @Output() switchTabsEvent = new EventEmitter<string>();
@Input() matchNumber: number = 1;
@Input() teamNumber: number = 1;
@Input() setNumber: number = 1;
@Input() compLevel: CompLevel = 'qm';
+ @Input() skipTeamSelection = false;
actionList: ActionT[] = [];
errorMessage: string = '';
@@ -119,6 +119,12 @@
matchStartTimestamp: number = 0;
+ ngOnInit() {
+ // When the user navigated from the match list, we can skip the team
+ // selection. I.e. we trust that the user clicked the correct button.
+ this.section = this.skipTeamSelection ? 'Init' : 'Team Selection';
+ }
+
addAction(action: ActionT): void {
if (action.type == 'startMatchAction') {
// Unix nanosecond timestamp.
@@ -301,7 +307,7 @@
builder.finish(SubmitActions.endSubmitActions(builder));
const buffer = builder.asUint8Array();
- const res = await fetch('/requests/submit/actions', {
+ const res = await fetch('/requests/submit/submit_actions', {
method: 'POST',
body: buffer,
});
diff --git a/scouting/www/entry/entry.ng.html b/scouting/www/entry/entry.ng.html
index e304f2a..56056fc 100644
--- a/scouting/www/entry/entry.ng.html
+++ b/scouting/www/entry/entry.ng.html
@@ -1,5 +1,8 @@
<div class="header" #header>
- <h2>{{section}}</h2>
+ <h2>
+ <span *ngIf="section != 'Team Selection'">{{teamNumber}}</span>
+ {{section}}
+ </h2>
</div>
<ng-container [ngSwitch]="section">
<div
@@ -89,7 +92,13 @@
<h6 class="text-muted">
Last Action: {{actionList[actionList.length - 1].type}}
</h6>
- <div class="d-grid gap-5">
+ <!--
+ Decrease distance between buttons during auto to make space for auto balancing
+ selection and keep all buttons visible without scrolling on most devices.
+ -->
+ <div
+ [ngClass]="{'d-grid': true, 'gap-3': autoPhase === true, 'gap-5': autoPhase === false}"
+ >
<button class="btn btn-secondary" (click)="undoLastAction()">UNDO</button>
<button
class="btn btn-danger"
@@ -133,7 +142,7 @@
</div>
<button
*ngIf="autoPhase"
- class="btn btn-info"
+ class="btn btn-dark"
(click)="autoPhase = false; addAction({type: 'endAutoPhase'});"
>
Start Teleop
@@ -151,7 +160,13 @@
<h6 class="text-muted">
Last Action: {{actionList[actionList.length - 1].type}}
</h6>
- <div class="d-grid gap-5">
+ <!--
+ Decrease distance between buttons during auto to make space for auto balancing
+ selection and keep all buttons visible without scrolling on most devices.
+ -->
+ <div
+ [ngClass]="{'d-grid': true, 'gap-3': autoPhase === true, 'gap-5': autoPhase === false}"
+ >
<button class="btn btn-secondary" (click)="undoLastAction()">UNDO</button>
<button
class="btn btn-danger"
@@ -177,6 +192,15 @@
>
LOW
</button>
+ <!-- Impossible to place supercharged pieces in auto. -->
+ <div *ngIf="autoPhase == false" class="d-grid gap-2">
+ <button
+ class="btn btn-dark"
+ (click)="changeSectionTo('Pickup'); addAction({type: 'placeObjectAction', scoreLevel: ScoreLevel.kSupercharged});"
+ >
+ SUPERCHARGED
+ </button>
+ </div>
<!-- 'Balancing' during auto. -->
<div *ngIf="autoPhase" class="d-grid gap-2">
<label>
@@ -201,7 +225,7 @@
</div>
<button
*ngIf="autoPhase"
- class="btn btn-info"
+ class="btn btn-dark"
(click)="autoPhase = false; addAction({type: 'endAutoPhase'});"
>
Start Teleop
diff --git a/scouting/www/notes/notes.component.ts b/scouting/www/notes/notes.component.ts
index 5d1c4a2..2786717 100644
--- a/scouting/www/notes/notes.component.ts
+++ b/scouting/www/notes/notes.component.ts
@@ -40,7 +40,7 @@
interface Keywords {
goodDriving: boolean;
badDriving: boolean;
- solidPickup: boolean;
+ solidPlacing: boolean;
sketchyPlacing: boolean;
goodDefense: boolean;
badDefense: boolean;
@@ -56,7 +56,7 @@
const KEYWORD_CHECKBOX_LABELS = {
goodDriving: 'Good Driving',
badDriving: 'Bad Driving',
- solidPickup: 'Solid Pickup',
+ solidPlacing: 'Solid Placing',
sketchyPlacing: 'Sketchy Placing',
goodDefense: 'Good Defense',
badDefense: 'Bad Defense',
@@ -115,7 +115,7 @@
keywordsData: {
goodDriving: false,
badDriving: false,
- solidPickup: false,
+ solidPlacing: false,
sketchyPlacing: false,
goodDefense: false,
badDefense: false,
@@ -152,7 +152,7 @@
dataFb,
this.newData[i].keywordsData.goodDriving,
this.newData[i].keywordsData.badDriving,
- this.newData[i].keywordsData.solidPickup,
+ this.newData[i].keywordsData.solidPlacing,
this.newData[i].keywordsData.sketchyPlacing,
this.newData[i].keywordsData.goodDefense,
this.newData[i].keywordsData.badDefense,
diff --git a/scouting/www/view/view.component.ts b/scouting/www/view/view.component.ts
index f7d6ce8..561ae08 100644
--- a/scouting/www/view/view.component.ts
+++ b/scouting/www/view/view.component.ts
@@ -171,8 +171,8 @@
if (entry.badDriving()) {
parsedKeywords += 'Bad Driving ';
}
- if (entry.solidPickup()) {
- parsedKeywords += 'Solid Pickup ';
+ if (entry.solidPlacing()) {
+ parsedKeywords += 'Solid Placing ';
}
if (entry.sketchyPlacing()) {
parsedKeywords += 'Sketchy Placing ';
diff --git a/third_party/libedgetpu/libedgetpu.BUILD b/third_party/libedgetpu/libedgetpu.BUILD
index 0289452..181c93a 100644
--- a/third_party/libedgetpu/libedgetpu.BUILD
+++ b/third_party/libedgetpu/libedgetpu.BUILD
@@ -1,11 +1,27 @@
cc_library(
- visibility = ["//visibility:public"],
name = "libedgetpu-k8",
- srcs = ["k8/libedgetpu.so.1.0"]
+ srcs = ["k8/libedgetpu.so.1.0"],
+ hdrs = glob(["include/**/*.h"]),
+ strip_include_prefix = "include",
+ visibility = ["//visibility:public"],
)
cc_library(
- visibility = ["//visibility:public"],
name = "libedgetpu-arm",
- srcs = ["arm/libedgetpu.so.1.0"]
-)
\ No newline at end of file
+ srcs = ["arm/libedgetpu.so.1.0"],
+ hdrs = glob(["include/**/*.h"]),
+ strip_include_prefix = "include",
+ visibility = ["//visibility:public"],
+)
+
+genrule(
+ name = "renamed_libedgetpu-arm",
+ srcs = [
+ "arm/libedgetpu.so.1.0",
+ ],
+ outs = [
+ "arm/libedgetpu.so.1",
+ ],
+ cmd = "cp $< $@",
+ visibility = ["//visibility:public"],
+)
diff --git a/third_party/libedgetpu/libedgetpu_build_script.sh b/third_party/libedgetpu/libedgetpu_build_script.sh
old mode 100644
new mode 100755
index 0eafccf..76e20db
--- a/third_party/libedgetpu/libedgetpu_build_script.sh
+++ b/third_party/libedgetpu/libedgetpu_build_script.sh
@@ -1,10 +1,18 @@
+#!/bin/bash
+#This script creates a compressed tarball file named libedgetpu-${GIT_VERSION}.tar.gz,
+# which contains the header files, libraries, and binaries needed to use Edge TPU on both arm and x86 architectures.
+# This script assumes you have Docker installed.
+#
# Clone the correct version of libedgetpu
git clone https://github.com/google-coral/libedgetpu.git
cd libedgetpu
+GIT_VERSION=ddfa7bde33c23afd8c2892182faa3e5b4e6ad94e
+git checkout ${GIT_VERSION}
# Build libedgetpu.so.1.0 for both arm and x86
DOCKER_CPUS="k8" DOCKER_IMAGE="ubuntu:18.04" DOCKER_TARGETS=libedgetpu make docker-build
DOCKER_CPUS="aarch64" DOCKER_IMAGE="debian:stretch" DOCKER_TARGETS=libedgetpu make docker-build
-# Create the directory for the tarball and move the resulting files into it
+# Create the directory for the tarball and move the resulting files into it
+rm -rf libedgetpu-bazel
mkdir libedgetpu-bazel
mkdir libedgetpu-bazel/arm
mkdir libedgetpu-bazel/k8
@@ -12,5 +20,7 @@
cp out/direct/k8/libedgetpu.so.1.0 libedgetpu-bazel/k8
# Copy header files to the include directory
-mkdir libedgetpu-bazel/include
-cp -r include/* libedgetpu-bazel/include/
+mkdir -p libedgetpu-bazel/include/tflite/
+rsync -zarv --include="*/" --include='*.h' --exclude='*' tflite/ libedgetpu-bazel/include/tflite/
+tar zcvf libedgetpu-${GIT_VERSION}.tar.gz libedgetpu-bazel
+
diff --git a/third_party/libtensorflowlite/libtensorflowlite.BUILD b/third_party/libtensorflowlite/libtensorflowlite.BUILD
index a6b837d..761ca76 100644
--- a/third_party/libtensorflowlite/libtensorflowlite.BUILD
+++ b/third_party/libtensorflowlite/libtensorflowlite.BUILD
@@ -1,16 +1,19 @@
cc_library(
- visibility = ["//visibility:public"],
name = "tensorflow-k8",
+ srcs = ["k8/libtensorflowlite.so"],
hdrs = glob(["include/**/*.h"]),
+ copts = ["-Wno-unused-parameter"],
strip_include_prefix = "include",
- srcs = ["k8/libtensorflowlite.so"]
+ visibility = ["//visibility:public"],
)
cc_library(
- visibility = ["//visibility:public"],
name = "tensorflow-arm",
+ srcs = ["arm/libtensorflowlite.so"],
hdrs = glob(["include/**/*.h"]),
+ copts = ["-Wno-unused-parameter"],
strip_include_prefix = "include",
- srcs = ["arm/libtensorflowlite.so"]
+ visibility = ["//visibility:public"],
)
+exports_files(["arm/libtensorflowlite.so"])
diff --git a/third_party/libtensorflowlite/tensorflow_build_script.sh b/third_party/libtensorflowlite/tensorflow_build_script.sh
old mode 100644
new mode 100755
index 6c44353..8bde976
--- a/third_party/libtensorflowlite/tensorflow_build_script.sh
+++ b/third_party/libtensorflowlite/tensorflow_build_script.sh
@@ -1,11 +1,18 @@
+#!/bin/bash
+# This script creates a compressed tarball file named tensorflow-${GIT_VERSION}.tar.gz,
+# which contains the header files, libraries, and binaries needed to use Tensorflow Lite on both arm and x86 architectures.
+# This script assumes you have bazelisk and necessary permissions.
+#
# Clone and checkout the correct version of Tensorflow
git clone https://github.com/tensorflow/tensorflow.git tensorflow_src
cd tensorflow_src
-git checkout v2.8.0
+GIT_VERSION=a4dfb8d1a71385bd6d122e4f27f86dcebb96712d
+git checkout $GIT_VERSION
# Build libtensorflowlite.so for both arm and x86
-bazel build --config=elinux_aarch64 -c opt //tensorflow/lite:libtensorflowlite.so
-bazel build --config=native_arch_linux -c opt //tensorflow/lite:libtensorflowlite.so
-# Create the directory for the tarball and move the resulting files into it
+bazelisk build --config=elinux_aarch64 -c opt //tensorflow/lite:libtensorflowlite.so
+bazelisk build --config=native_arch_linux -c opt //tensorflow/lite:libtensorflowlite.so
+# Create the directory for the tarball and move the resulting files into it
+rm -rf tensorflow-bazel
mkdir tensorflow-bazel
mkdir tensorflow-bazel/arm
mkdir tensorflow-bazel/k8
@@ -13,5 +20,8 @@
cp bazel-out/k8-opt/bin/tensorflow/lite/libtensorflowlite.so tensorflow-bazel/k8
# Copy header files to the include directory
- mkdir -p tensorflow-bazel/tensorflow/core/util
- rsync -zarv --include='*/' --include='*.h' --exclude='*' tensorflow/core/util tensorflow-bazel/tensorflow/core/util
\ No newline at end of file
+mkdir -p tensorflow-bazel/include/tensorflow/
+mkdir -p tensorflow-bazel/include/flatbuffers/
+rsync -zarv --include="*/" --include='*.h' --exclude='*' tensorflow/ tensorflow-bazel/include/tensorflow/
+rsync -zarv --include="*/" --include='*.h' --exclude='*' bazel-out/../../../external/flatbuffers/include/flatbuffers/ tensorflow-bazel/include/flatbuffers/
+tar zcvf tensorflow-${GIT_VERSION}.tar.gz tensorflow-bazel
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/y2022_bot3/control_loops/drivetrain/drivetrain_base.cc b/y2022_bot3/control_loops/drivetrain/drivetrain_base.cc
index 8fd6d94..a97e0dd 100644
--- a/y2022_bot3/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2022_bot3/control_loops/drivetrain/drivetrain_base.cc
@@ -27,7 +27,7 @@
static constexpr double kImuYaw = 0.0;
static DrivetrainConfig<double> kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
- ::frc971::control_loops::drivetrain::LoopType::OPEN_LOOP,
+ ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
diff --git a/y2022_bot3/control_loops/python/drivetrain.py b/y2022_bot3/control_loops/python/drivetrain.py
index 638b3f3..7db0a05 100644
--- a/y2022_bot3/control_loops/python/drivetrain.py
+++ b/y2022_bot3/control_loops/python/drivetrain.py
@@ -24,7 +24,7 @@
q_pos=0.24,
q_vel=2.5,
efficiency=0.80,
- has_imu=True,
+ has_imu=False,
force=True,
kf_q_voltage=1.0,
controller_poles=[0.82, 0.82])
diff --git a/y2022_bot3/joystick_reader.cc b/y2022_bot3/joystick_reader.cc
index 159123c..00a28be 100644
--- a/y2022_bot3/joystick_reader.cc
+++ b/y2022_bot3/joystick_reader.cc
@@ -42,7 +42,7 @@
: ::frc971::input::ActionJoystickInput(
event_loop,
::y2022_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
- ::frc971::input::DrivetrainInputReader::InputType::kSteeringWheel, {}),
+ ::frc971::input::DrivetrainInputReader::InputType::kPistol, {}),
superstructure_goal_sender_(
event_loop->MakeSender<superstructure::Goal>("/superstructure")),
superstructure_status_fetcher_(
diff --git a/y2022_bot3/wpilib_interface.cc b/y2022_bot3/wpilib_interface.cc
index ae838f9..b1fb08a 100644
--- a/y2022_bot3/wpilib_interface.cc
+++ b/y2022_bot3/wpilib_interface.cc
@@ -334,72 +334,6 @@
climber_encoder_left_, climber_encoder_right_;
};
-class SuperstructureWriter
- : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
- public:
- SuperstructureWriter(aos::EventLoop *event_loop)
- : frc971::wpilib::LoopOutputHandler<superstructure::Output>(
- event_loop, "/superstructure") {}
-
- void set_intake_roller_falcon(
- ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
- intake_roller_falcon_ = std::move(t);
- intake_roller_falcon_->ConfigSupplyCurrentLimit(
- {true, Values::kIntakeRollerSupplyCurrentLimit(),
- Values::kIntakeRollerSupplyCurrentLimit(), 0});
- intake_roller_falcon_->ConfigStatorCurrentLimit(
- {true, Values::kIntakeRollerStatorCurrentLimit(),
- Values::kIntakeRollerStatorCurrentLimit(), 0});
- }
-
- void set_climber_falcon_left(std::unique_ptr<frc::TalonFX> t) {
- climber_falcon_left_ = std::move(t);
- }
-
- void set_climber_falcon_right(std::unique_ptr<frc::TalonFX> t) {
- climber_falcon_right_ = std::move(t);
- }
-
- void set_intake_falcon(std::unique_ptr<frc::TalonFX> t) {
- intake_falcon_ = std::move(t);
- }
-
- private:
- void Stop() override {
- AOS_LOG(WARNING, "Superstructure output too old.\n");
- climber_falcon_left_->SetDisabled();
- climber_falcon_right_->SetDisabled();
- intake_falcon_->SetDisabled();
- intake_roller_falcon_->Set(
- ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
- }
-
- void Write(const superstructure::Output &output) override {
- WritePwm(output.climber_voltage_left(), climber_falcon_left_.get());
- WritePwm(output.climber_voltage_right(), climber_falcon_right_.get());
- WritePwm(output.intake_voltage(), intake_falcon_.get());
- WriteCan(output.roller_voltage(), intake_roller_falcon_.get());
- }
-
- static void WriteCan(const double voltage,
- ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
- falcon->Set(
- ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
- std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
- }
-
- template <typename T>
- static void WritePwm(const double voltage, T *motor) {
- motor->SetSpeed(std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
- }
-
- ::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
- intake_roller_falcon_;
- ::std::unique_ptr<::frc::TalonFX> climber_falcon_left_, climber_falcon_right_,
- intake_falcon_;
-};
-
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
::std::unique_ptr<frc::Encoder> make_encoder(int index) {
@@ -429,11 +363,11 @@
::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop, values);
sensor_reader.set_pwm_trigger(true);
- sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
- sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
- sensor_reader.set_heading_input(make_unique<frc::DigitalInput>(9));
- sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(8));
+ sensor_reader.set_heading_input(make_unique<frc::DigitalInput>(8));
+ sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(9));
sensor_reader.set_intake_encoder(make_encoder(2));
sensor_reader.set_climber_encoder_right(make_encoder(3));
@@ -458,16 +392,12 @@
::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), false);
+ drivetrain_writer.set_left_controller1(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
drivetrain_writer.set_right_controller0(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), true);
-
- SuperstructureWriter superstructure_writer(&output_event_loop);
- superstructure_writer.set_climber_falcon_left(make_unique<frc::TalonFX>(2));
- superstructure_writer.set_climber_falcon_right(
- make_unique<frc::TalonFX>(3));
- superstructure_writer.set_intake_falcon(make_unique<frc::TalonFX>(4));
- superstructure_writer.set_intake_roller_falcon(
- make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)), true);
+ drivetrain_writer.set_right_controller1(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)), true);
AddLoop(&output_event_loop);
// Thread 5.
diff --git a/y2023/BUILD b/y2023/BUILD
index 08713b7..6be5cac 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -55,6 +55,7 @@
"//aos/util:foxglove_websocket",
"//y2023/vision:viewer",
"//y2023/vision:localization_verifier",
+ "//y2023/vision:camera_monitor",
"//y2023/vision:aprilrobotics",
"//aos/events:aos_timing_report_streamer",
"//y2023/localizer:localizer_main",
@@ -70,8 +71,12 @@
":aos_config",
"//frc971/rockpi:rockpi_config.json",
"//y2023/constants:constants.json",
+ "//y2023/vision:game_pieces_detector_starter",
"//y2023/vision:image_streamer_start",
"//y2023/www:www_files",
+ "@game_pieces_edgetpu_model//file",
+ "@libedgetpu//:arm/libedgetpu.so.1",
+ "@libtensorflowlite//:arm/libtensorflowlite.so",
],
dirs = [
"//y2023/www:www_files",
@@ -322,6 +327,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/autonomous/auto_splines.cc b/y2023/autonomous/auto_splines.cc
index c99e417..20593f9 100644
--- a/y2023/autonomous/auto_splines.cc
+++ b/y2023/autonomous/auto_splines.cc
@@ -142,6 +142,46 @@
alliance);
}
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineCable1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(splinecable_1_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineCable2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(splinecable_2_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineCable3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(splinecable_3_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::SplineCable4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(splinecable_4_, builder->fbb()),
+ alliance);
+}
+
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
diff --git a/y2023/autonomous/auto_splines.h b/y2023/autonomous/auto_splines.h
index 27442f3..6921e64 100644
--- a/y2023/autonomous/auto_splines.h
+++ b/y2023/autonomous/auto_splines.h
@@ -4,7 +4,6 @@
#include "aos/events/event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "frc971/control_loops/control_loops_generated.h"
-#include "frc971/input/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
#include "frc971/input/joystick_state_generated.h"
/*
@@ -29,7 +28,15 @@
spline_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
"splines/spline.2.json")),
spline_4_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/spline.3.json")) {}
+ "splines/spline.3.json")),
+ splinecable_1_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/splinecable.0.json")),
+ splinecable_2_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/splinecable.1.json")),
+ splinecable_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/splinecable.2.json")),
+ splinecable_4_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/splinecable.3.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
@@ -59,6 +66,22 @@
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> SplineCable1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> SplineCable2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> SplineCable3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> SplineCable4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
private:
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
@@ -66,6 +89,10 @@
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_2_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_3_;
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_4_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> splinecable_1_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> splinecable_2_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> splinecable_3_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> splinecable_4_;
};
} // namespace autonomous
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index 26197e3..cd4d4f1 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -14,6 +14,7 @@
DEFINE_bool(spline_auto, false, "Run simple test S-spline auto mode.");
DEFINE_bool(charged_up, true, "If true run charged up autonomous mode");
+DEFINE_bool(charged_up_cable, false, "If true run cable side autonomous mode");
namespace y2023 {
namespace autonomous {
@@ -126,11 +127,28 @@
SplineDirection::kBackward),
PlanSpline(std::bind(&AutonomousSplines::Spline4, &auto_splines_,
std::placeholders::_1, alliance_),
- SplineDirection::kForward),
- };
+ SplineDirection::kForward)};
starting_position_ = charged_up_splines_.value()[0].starting_position();
CHECK(starting_position_);
+ } else if (FLAGS_charged_up_cable) {
+ charged_up_cable_splines_ = {
+ PlanSpline(std::bind(&AutonomousSplines::SplineCable1, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ PlanSpline(std::bind(&AutonomousSplines::SplineCable2, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(std::bind(&AutonomousSplines::SplineCable3, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ PlanSpline(std::bind(&AutonomousSplines::SplineCable4, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward)};
+
+ starting_position_ =
+ charged_up_cable_splines_.value()[0].starting_position();
+ CHECK(starting_position_);
}
is_planned_ = true;
@@ -188,6 +206,8 @@
SplineAuto();
} else if (FLAGS_charged_up) {
ChargedUp();
+ } else if (FLAGS_charged_up_cable) {
+ ChargedUpCableSide();
} else {
AOS_LOG(WARNING, "No auto mode selected.");
}
@@ -222,7 +242,7 @@
}
}
-// Charged Up 3 Game Object Autonomous.
+// Charged Up 3 Game Object Autonomous (non-cable side)
void AutonomousActor::ChargedUp() {
aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
@@ -401,6 +421,143 @@
}
}
+// Charged Up 3 Game Object Autonomous (cable side)
+void AutonomousActor::ChargedUpCableSide() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+
+ CHECK(charged_up_cable_splines_);
+
+ auto &splines = *charged_up_cable_splines_;
+
+ AOS_LOG(INFO, "Going to preload");
+
+ // Tell the superstructure a cone was preloaded
+ if (!WaitForPreloaded()) return;
+ AOS_LOG(INFO, "Moving arm");
+
+ // Place first cone on mid level
+ MidConeScore();
+
+ // Wait until the arm is at the goal to spit
+ if (!WaitForArmGoal(0.10)) return;
+ Spit();
+ if (!WaitForArmGoal(0.01)) return;
+
+ std::this_thread::sleep_for(chrono::milliseconds(100));
+
+ AOS_LOG(
+ INFO, "Placed first cone %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ // Drive and intake the cube nearest to the starting zone
+ if (!splines[0].WaitForPlan()) return;
+ splines[0].Start();
+
+ // Move arm into position to pickup a cube and start cube intake
+ PickupCube();
+
+ std::this_thread::sleep_for(chrono::milliseconds(500));
+
+ IntakeCube();
+
+ AOS_LOG(
+ INFO, "Turning on rollers %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
+
+ AOS_LOG(
+ INFO, "Got there %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ // Drive back to grid and place cube on high level
+ if (!splines[1].WaitForPlan()) return;
+ splines[1].Start();
+
+ std::this_thread::sleep_for(chrono::milliseconds(300));
+ Neutral();
+
+ if (!splines[1].WaitForSplineDistanceTraveled(3.2)) return;
+ HighCubeScore();
+ AOS_LOG(
+ INFO, "Extending arm %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!splines[1].WaitForSplineDistanceRemaining(0.08)) return;
+ AOS_LOG(
+ INFO, "Back for first cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!WaitForArmGoal(0.10)) return;
+
+ AOS_LOG(
+ INFO, "Arm in place for first cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ Spit();
+
+ if (!splines[1].WaitForSplineDistanceRemaining(0.08)) return;
+
+ AOS_LOG(
+ INFO, "Finished spline back %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!WaitForArmGoal(0.05)) return;
+
+ AOS_LOG(
+ INFO, "Placed first cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ // Drive and intake the cube second nearest to the starting zone
+ if (!splines[2].WaitForPlan()) return;
+ splines[2].Start();
+
+ std::this_thread::sleep_for(chrono::milliseconds(200));
+ PickupCube();
+
+ std::this_thread::sleep_for(chrono::milliseconds(500));
+ IntakeCube();
+
+ if (!splines[2].WaitForSplineDistanceRemaining(0.05)) return;
+ AOS_LOG(
+ INFO, "Picked up second cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ // Drive back to grid and place object on mid level
+ if (!splines[3].WaitForPlan()) return;
+ splines[3].Start();
+
+ std::this_thread::sleep_for(chrono::milliseconds(400));
+ Neutral();
+
+ AOS_LOG(
+ INFO, "Driving back %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!splines[3].WaitForSplineDistanceTraveled(3.5)) return;
+ AOS_LOG(
+ INFO, "Extending arm %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+ MidCubeScore();
+
+ if (!splines[3].WaitForSplineDistanceRemaining(0.07)) return;
+ AOS_LOG(
+ INFO, "Got back from second cube at %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ if (!WaitForArmGoal(0.05)) return;
+ Spit();
+
+ if (!splines[3].WaitForSplineDistanceRemaining(0.02)) return;
+
+ AOS_LOG(
+ INFO, "Placed second cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ std::this_thread::sleep_for(chrono::milliseconds(200));
+ Neutral();
+}
+
void AutonomousActor::SendSuperstructureGoal() {
auto builder = superstructure_goal_sender_.MakeBuilder();
diff --git a/y2023/autonomous/autonomous_actor.h b/y2023/autonomous/autonomous_actor.h
index 46c8a78..31ae4f5 100644
--- a/y2023/autonomous/autonomous_actor.h
+++ b/y2023/autonomous/autonomous_actor.h
@@ -58,6 +58,7 @@
void MaybeSendStartingPosition();
void SplineAuto();
void ChargedUp();
+ void ChargedUpCableSide();
void Replan();
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
@@ -90,6 +91,7 @@
std::optional<SplineHandle> test_spline_;
std::optional<std::array<SplineHandle, 4>> charged_up_splines_;
+ std::optional<std::array<SplineHandle, 4>> charged_up_cable_splines_;
// List of arm angles from arm::PointsList
const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
diff --git a/y2023/autonomous/splines/splinecable.0.json b/y2023/autonomous/splines/splinecable.0.json
new file mode 100644
index 0000000..3a8a1c8
--- /dev/null
+++ b/y2023/autonomous/splines/splinecable.0.json
@@ -0,0 +1 @@
+{"spline_count": 2, "spline_x": [6.468141183035714, 6.156227508178545, 5.627928372772197, 5.618126908868124, 4.926607785226736, 4.384253622459038, 3.8418994596913407, 3.448710257797334, 2.6702357572801336, 1.9707990587626902, 1.441511105732058], "spline_y": [-3.493364620535714, -3.4921188608837532, -3.431437306632174, -3.364982889102878, -3.381212722774612, -3.3815433034902798, -3.3818738842059477, -3.3663052119655514, -3.293883383936489, -3.2050645805145246, -3.125670625960137], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 12.0}, {"constraint_type": "VELOCITY", "value": 1.5, "start_distance": 1.6, "end_distance": 2.75}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/splinecable.1.json b/y2023/autonomous/splines/splinecable.1.json
new file mode 100644
index 0000000..bf6838d
--- /dev/null
+++ b/y2023/autonomous/splines/splinecable.1.json
@@ -0,0 +1 @@
+{"spline_count": 2, "spline_x": [1.441511105732058, 2.158787794162399, 2.644327791138681, 3.0017835551344163, 3.810734918966552, 4.3709130304548385, 4.931091141943125, 5.242496001087563, 5.544694674297983, 5.965729060465805, 6.444059695844368], "spline_y": [-3.125670625960137, -3.233263161324085, -3.2028042659537914, -3.139500351375334, -3.125566969972238, -3.126688937308022, -3.127810904643806, -3.143988220718469, -2.9893341041515376, -2.943814113563144, -2.9458417329239843], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 12.0}, {"constraint_type": "VELOCITY", "value": 1.5, "start_distance": 1.9, "end_distance": 10.0}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/splinecable.2.json b/y2023/autonomous/splines/splinecable.2.json
new file mode 100644
index 0000000..1caf288
--- /dev/null
+++ b/y2023/autonomous/splines/splinecable.2.json
@@ -0,0 +1 @@
+{"spline_count": 2, "spline_x": [6.444059695844368, 5.977202894617023, 5.571532532121525, 5.64631894401731, 5.012590627058541, 4.364949107965309, 3.7173075888720764, 3.0557528676443795, 2.3031869947159427, 1.8583292479008144, 1.389061484581065], "spline_y": [-2.9458417329239843, -2.943862750565559, -2.96219050239874, -3.128181973537357, -3.0803143268366417, -3.079651639654858, -3.0789889524730745, -3.1255312248102225, -2.6948472310294784, -2.234117571800885, -1.9039073778940578], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 12.0}, {"constraint_type": "VELOCITY", "value": 1.5, "start_distance": 1.6, "end_distance": 2.55}]}
\ No newline at end of file
diff --git a/y2023/autonomous/splines/splinecable.3.json b/y2023/autonomous/splines/splinecable.3.json
new file mode 100644
index 0000000..c49c1e8
--- /dev/null
+++ b/y2023/autonomous/splines/splinecable.3.json
@@ -0,0 +1 @@
+{"spline_count": 2, "spline_x": [1.389061484581065, 2.1138941612201396, 2.2865982233539297, 3.1216159825232483, 3.763030179317507, 4.301019167021117, 4.839008154724727, 5.2735719333376885, 5.59798451182913, 5.734162859875811, 6.443454541551164], "spline_y": [-1.9039073778940578, -2.4139512321954397, -2.680729209318427, -3.1198721507158496, -3.0738965122418085, -3.0726645678919953, -3.071432623542182, -3.1149443733165967, -2.966418143397661, -2.9578217694545557, -2.957336170980663], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 12.0}, {"constraint_type": "VELOCITY", "value": 1.5, "start_distance": 2.5, "end_distance": 3.6}]}
\ No newline at end of file
diff --git a/y2023/constants.cc b/y2023/constants.cc
index 07b2ee6..3d3dfc2 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -83,24 +83,25 @@
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 + 0.5153995156153;
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 +109,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;
+ 0.858579506535361;
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.79390317510529;
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;
+ 4.68612810338484;
break;
diff --git a/y2023/constants.h b/y2023/constants.h
index f404472..32ed317 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);
}
diff --git a/y2023/constants/7971.json b/y2023/constants/7971.json
index a3bd130..74fd3aa 100644
--- a/y2023/constants/7971.json
+++ b/y2023/constants/7971.json
@@ -4,10 +4,10 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-1_cam-23-01_ext_2023-02-19.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-02-19.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-08.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-02-19.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-08.json' %}
},
{
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-4_cam-23-04_ext_2023-02-19.json' %}
diff --git a/y2023/constants/971.json b/y2023/constants/971.json
index 35a9a1e..b9351fd 100644
--- a/y2023/constants/971.json
+++ b/y2023/constants/971.json
@@ -7,7 +7,7 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-2_cam-23-06_ext_2023-03-25.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-03-26.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-3_cam-23-04_ext_2023-04-10.json' %}
},
{
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_ext_2023-02-22.json' %}
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index bbc7a1b..99b09af 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -244,6 +244,7 @@
self.set_size_request(ARM_AREA_WIDTH, ARM_AREA_HEIGHT)
self.center = (0, 0)
self.shape = (ARM_AREA_WIDTH, ARM_AREA_HEIGHT)
+ self.window_shape = (ARM_AREA_WIDTH, ARM_AREA_HEIGHT)
self.theta_version = False
self.init_extents()
@@ -326,9 +327,6 @@
def on_draw(self, widget, event):
cr = self.get_window().cairo_create()
- self.window_shape = (self.get_window().get_geometry().width,
- self.get_window().get_geometry().height)
-
cr.save()
cr.set_font_size(20)
cr.translate(self.window_shape[0] / 2, self.window_shape[1] / 2)
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 93134c7..1efcd93 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -329,7 +329,7 @@
))
points['HPPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
- -1.1200539, 1.330, np.pi / 2.0, circular_index=0)
+ -1.1200539, 1.325, np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
diff --git a/y2023/control_loops/superstructure/arm/trajectory.cc b/y2023/control_loops/superstructure/arm/trajectory.cc
index d216467..88e3d1b 100644
--- a/y2023/control_loops/superstructure/arm/trajectory.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory.cc
@@ -974,8 +974,8 @@
}
// Pull us back to the previous point until we aren't saturated anymore.
- double saturation_goal_velocity;
- double saturation_goal_acceleration;
+ double saturation_goal_velocity = 0.0;
+ double saturation_goal_acceleration = 0.0;
while (step_size > 0.01) {
USaturationSearch(goal_(0), last_goal_(0), goal_(1), last_goal_(1),
saturation_fraction_along_path_, arm_K, X, *trajectory_,
diff --git a/y2023/control_loops/superstructure/arm/trajectory_plot.cc b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
index c56ced9..24bc8dd 100644
--- a/y2023/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
@@ -15,10 +15,10 @@
DEFINE_bool(plot, true, "If true, plot");
DEFINE_bool(plot_thetas, true, "If true, plot the angles");
-DEFINE_double(alpha0_max, 20.0, "Max acceleration on joint 0.");
-DEFINE_double(alpha1_max, 30.0, "Max acceleration on joint 1.");
-DEFINE_double(alpha2_max, 60.0, "Max acceleration on joint 2.");
-DEFINE_double(vmax_plan, 10.0, "Max voltage to plan.");
+DEFINE_double(alpha0_max, 15.0, "Max acceleration on joint 0.");
+DEFINE_double(alpha1_max, 10.0, "Max acceleration on joint 1.");
+DEFINE_double(alpha2_max, 90.0, "Max acceleration on joint 2.");
+DEFINE_double(vmax_plan, 9.5, "Max voltage to plan.");
DEFINE_double(vmax_battery, 12.0, "Max battery voltage.");
DEFINE_double(time, 2.0, "Simulation time.");
@@ -36,12 +36,20 @@
Eigen::Matrix<double, 2, 4> spline_params;
- spline_params << 0.30426338, 0.42813912, 0.64902386, 0.55127045, -1.73611082,
- -1.64478944, -1.04763868, -0.82624244;
+ spline_params << 3.227752818257, 3.032002509469, 3.131082488348,
+ 3.141592653590, 0.914286433787, 0.436747899287, 0.235917057271,
+ 0.000000000000;
LOG(INFO) << "Spline " << spline_params;
NSpline<4, 2> spline(spline_params);
CosSpline cos_spline(spline,
- {{0.0, 0.1}, {0.3, 0.1}, {0.7, 0.2}, {1.0, 0.2}});
+ {
+ CosSpline::AlphaTheta{.alpha = 0.000000000000,
+ .theta = 1.570796326795},
+ CosSpline::AlphaTheta{.alpha = 0.050000000000,
+ .theta = 1.570796326795},
+ CosSpline::AlphaTheta{.alpha = 1.000000000000,
+ .theta = 0.000000000000},
+ });
Path distance_spline(cos_spline, 100);
Trajectory trajectory(&dynamics, &hybrid_roll.plant(),
@@ -300,10 +308,14 @@
(::Eigen::Matrix<double, 2, 1>() << arm_X(0), arm_X(2)).finished(),
sim_dt);
roll.Correct((::Eigen::Matrix<double, 1, 1>() << roll_X(0)).finished());
+ bool disabled = false;
+ if (t > 0.40 && t < 0.46) {
+ disabled = true;
+ }
follower.Update(
(Eigen::Matrix<double, 9, 1>() << arm_ekf.X_hat(), roll.X_hat())
.finished(),
- false, sim_dt, FLAGS_vmax_plan, FLAGS_vmax_battery);
+ disabled, sim_dt, FLAGS_vmax_plan, FLAGS_vmax_battery);
const ::Eigen::Matrix<double, 3, 1> theta_t =
trajectory.ThetaT(follower.goal()(0));
diff --git a/y2023/control_loops/superstructure/led_indicator.cc b/y2023/control_loops/superstructure/led_indicator.cc
index 5cee266..d67bdc2 100644
--- a/y2023/control_loops/superstructure/led_indicator.cc
+++ b/y2023/control_loops/superstructure/led_indicator.cc
@@ -49,29 +49,38 @@
static_cast<int>(b));
}
-namespace {
-bool DisconnectedPiServer(
- const aos::message_bridge::ServerStatistics &server_stats) {
- for (const auto *pi_server_status : *server_stats.connections()) {
- if (pi_server_status->state() == aos::message_bridge::State::DISCONNECTED &&
- pi_server_status->node()->name()->string_view() != "logger") {
+bool DisconnectedIMUPiServer(
+ const aos::message_bridge::ServerStatistics &server_statistics) {
+ for (const auto *node_status : *server_statistics.connections()) {
+ if (node_status->state() == aos::message_bridge::State::DISCONNECTED &&
+ node_status->node()->name()->string_view() != "logger") {
return true;
}
}
+
return false;
}
-bool DisconnectedPiClient(
- const aos::message_bridge::ClientStatistics &client_stats) {
- for (const auto *pi_client_status : *client_stats.connections()) {
- if (pi_client_status->state() == aos::message_bridge::State::DISCONNECTED &&
- pi_client_status->node()->name()->string_view() != "logger") {
+bool DisconnectedIMUPiClient(
+ const aos::message_bridge::ClientStatistics &client_statistics) {
+ for (const auto *node_status : *client_statistics.connections()) {
+ if (node_status->state() == aos::message_bridge::State::DISCONNECTED &&
+ node_status->node()->name()->string_view() != "logger") {
return true;
}
}
+
return false;
}
-} // namespace
+
+bool PisDisconnected(
+ const frc971::controls::LocalizerOutput &localizer_output) {
+ if (!localizer_output.all_pis_connected()) {
+ return true;
+ } else {
+ return false;
+ }
+}
void LedIndicator::DecideColor() {
superstructure_status_fetcher_.Fetch();
@@ -122,11 +131,12 @@
return;
}
- // Pi disconnected
- if ((server_statistics_fetcher_.get() &&
- DisconnectedPiServer(*server_statistics_fetcher_)) ||
- (client_statistics_fetcher_.get() &&
- DisconnectedPiClient(*client_statistics_fetcher_))) {
+ if (localizer_output_fetcher_.get() == nullptr ||
+ server_statistics_fetcher_.get() == nullptr ||
+ client_statistics_fetcher_.get() == nullptr ||
+ PisDisconnected(*localizer_output_fetcher_) ||
+ DisconnectedIMUPiServer(*server_statistics_fetcher_) ||
+ DisconnectedIMUPiClient(*client_statistics_fetcher_)) {
if (flash_counter_.Flash()) {
DisplayLed(255, 0, 0);
} else {
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index ece0790..4919cd6 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -132,7 +132,7 @@
break;
}
constexpr double kInvalidReading = 0.93;
- if (reading > kInvalidReading) {
+ if (reading > kInvalidReading || !std::isfinite(reading)) {
return std::nullopt;
}
const TimeOfFlight *calibration = CHECK_NOTNULL(
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 80e41f9..b555277 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -451,7 +451,6 @@
SetEnabled(true);
WaitUntilZeroed();
-
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
@@ -605,23 +604,44 @@
RunFor(chrono::seconds(1));
// Game piece position should not be populated when invalid.
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_EQ(vision::Class::CONE_UP,
+ superstructure_status_fetcher_->game_piece());
EXPECT_FALSE(superstructure_status_fetcher_->has_game_piece_position());
// And then send a valid cone position.
superstructure_plant_.set_cone_position(0.5);
RunFor(chrono::seconds(1));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_EQ(vision::Class::CONE_UP,
+ superstructure_status_fetcher_->game_piece());
EXPECT_TRUE(superstructure_status_fetcher_->has_game_piece_position());
EXPECT_FLOAT_EQ(0.0, superstructure_status_fetcher_->game_piece_position());
superstructure_plant_.set_cone_position(0.1);
RunFor(chrono::seconds(1));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_EQ(vision::Class::CONE_UP,
+ superstructure_status_fetcher_->game_piece());
EXPECT_TRUE(superstructure_status_fetcher_->has_game_piece_position());
EXPECT_FLOAT_EQ(0.2, superstructure_status_fetcher_->game_piece_position());
+
+ // Test that if wpilib_interface spews out invalid values that we don't
+ // attempt to convert them.
+ superstructure_plant_.set_cone_position(
+ std::numeric_limits<double>::quiet_NaN());
+ RunFor(chrono::seconds(1));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(vision::Class::CONE_UP,
+ superstructure_status_fetcher_->game_piece());
+ EXPECT_FALSE(superstructure_status_fetcher_->has_game_piece_position());
+
+ superstructure_plant_.set_cone_position(
+ -std::numeric_limits<double>::infinity());
+ RunFor(chrono::seconds(1));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(vision::Class::CONE_UP,
+ superstructure_status_fetcher_->game_piece());
+ EXPECT_FALSE(superstructure_status_fetcher_->has_game_piece_position());
}
class SuperstructureBeambreakTest
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/BUILD b/y2023/localizer/BUILD
index 43c4481..3ef024c 100644
--- a/y2023/localizer/BUILD
+++ b/y2023/localizer/BUILD
@@ -146,6 +146,7 @@
":visualization_fbs",
"//aos/containers:sized_array",
"//aos/events:event_loop",
+ "//aos/network:message_bridge_client_fbs",
"//frc971/constants:constants_sender_lib",
"//frc971/control_loops:pose",
"//frc971/control_loops/drivetrain:hybrid_ekf",
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index c663954..502fbde 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -112,7 +112,13 @@
utils_(event_loop),
status_sender_(event_loop->MakeSender<Status>("/localizer")),
output_sender_(event_loop->MakeSender<frc971::controls::LocalizerOutput>(
- "/localizer")) {
+ "/localizer")),
+ server_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/aos")),
+ client_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ClientStatistics>(
+ "/aos")) {
if (dt_config_.is_simulated) {
down_estimator_.assume_perfect_gravity();
}
@@ -451,6 +457,34 @@
quaternion.mutate_z(orientation.z());
quaternion.mutate_w(orientation.w());
output_builder.add_orientation(&quaternion);
+ server_statistics_fetcher_.Fetch();
+ client_statistics_fetcher_.Fetch();
+
+ bool pis_connected = true;
+
+ if (server_statistics_fetcher_.get()) {
+ for (const auto *pi_server_status :
+ *server_statistics_fetcher_->connections()) {
+ if (pi_server_status->state() ==
+ aos::message_bridge::State::DISCONNECTED &&
+ pi_server_status->node()->name()->string_view() != "logger") {
+ pis_connected = false;
+ }
+ }
+ }
+
+ if (client_statistics_fetcher_.get()) {
+ for (const auto *pi_client_status :
+ *client_statistics_fetcher_->connections()) {
+ if (pi_client_status->state() ==
+ aos::message_bridge::State::DISCONNECTED &&
+ pi_client_status->node()->name()->string_view() != "logger") {
+ pis_connected = false;
+ }
+ }
+ }
+
+ output_builder.add_all_pis_connected(pis_connected);
builder.CheckOk(builder.Send(output_builder.Finish()));
}
diff --git a/y2023/localizer/localizer.h b/y2023/localizer/localizer.h
index 4b0715b..5190544 100644
--- a/y2023/localizer/localizer.h
+++ b/y2023/localizer/localizer.h
@@ -4,6 +4,8 @@
#include <array>
#include <map>
+#include "aos/network/message_bridge_client_generated.h"
+#include "aos/network/message_bridge_server_generated.h"
#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
@@ -117,6 +119,11 @@
// For the status message.
std::optional<Eigen::Vector2d> last_encoder_readings_;
+
+ aos::Fetcher<aos::message_bridge::ServerStatistics>
+ server_statistics_fetcher_;
+ aos::Fetcher<aos::message_bridge::ClientStatistics>
+ client_statistics_fetcher_;
};
} // namespace y2023::localizer
#endif // Y2023_LOCALIZER_LOCALIZER_H_
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/BUILD b/y2023/vision/BUILD
index 0f75aba..ca057fd 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -1,5 +1,6 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+load("//tools/build_rules:select.bzl", "cpu_select")
cc_binary(
name = "camera_reader",
@@ -223,11 +224,13 @@
],
data = [
"//y2023:aos_config",
+ "@game_pieces_edgetpu_model//file",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2023:__subpackages__"],
deps = [
":game_pieces_fbs",
+ ":yolov5_lib",
"//aos/events:event_loop",
"//aos/events:shm_event_loop",
"//frc971/vision:vision_fbs",
@@ -267,3 +270,53 @@
"@com_github_google_glog//:glog",
],
)
+
+cc_library(
+ name = "yolov5_lib",
+ srcs = ["yolov5.cc"],
+ hdrs = ["yolov5.h"],
+ copts = ["-Wno-unused-parameter"],
+ deps = [
+ "//third_party:opencv",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/types:span",
+ ] + cpu_select({
+ "amd64": [
+ "@libtensorflowlite//:tensorflow-k8",
+ "@libedgetpu//:libedgetpu-k8",
+ ],
+ "arm": [
+ "@libtensorflowlite//:tensorflow-arm",
+ "@libedgetpu//:libedgetpu-arm",
+ ],
+ }),
+)
+
+filegroup(
+ name = "game_pieces_detector_starter",
+ srcs = ["game_pieces_detector_starter.sh"],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "camera_monitor_lib",
+ srcs = ["camera_monitor_lib.cc"],
+ hdrs = ["camera_monitor_lib.h"],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/starter:starter_rpc_lib",
+ "//frc971/vision:vision_fbs",
+ ],
+)
+
+cc_binary(
+ name = "camera_monitor",
+ srcs = ["camera_monitor.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":camera_monitor_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 45ab7db..f2e2592 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -33,12 +33,13 @@
flip_image_(flip_image),
node_name_(event_loop->node()->name()->string_view()),
ftrace_(),
- image_callback_(event_loop, channel_name,
- [&](cv::Mat image_color_mat,
- const aos::monotonic_clock::time_point eof) {
- HandleImage(image_color_mat, eof);
- },
- chrono::milliseconds(5)),
+ image_callback_(
+ event_loop, channel_name,
+ [&](cv::Mat image_color_mat,
+ const aos::monotonic_clock::time_point eof) {
+ HandleImage(image_color_mat, eof);
+ },
+ chrono::milliseconds(5)),
target_map_sender_(
event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
image_annotations_sender_(
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-02-19.json b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-02-19.json
deleted file mode 100644
index addacf2..0000000
--- a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-02-19.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi2", "team_number": 7971, "intrinsics": [ 895.543945, 0.0, 645.250122, 0.0, 895.308838, 354.297241, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.513422, -0.00069, 0.858136, 0.132248, -0.858026, -0.015658, -0.513368, -0.002601, 0.013791, -0.999877, 0.007447, -0.001246, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.455658, 0.272167, 0.000796, -0.000206, -0.0975 ], "calibration_timestamp": 1358499769498335208, "camera_id": "23-02" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-05.json b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-05.json
new file mode 100644
index 0000000..b175abb
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-05.json
@@ -0,0 +1 @@
+{ "node_name": "pi2", "team_number": 7971, "intrinsics": [ 895.543945, 0.0, 645.250122, 0.0, 895.308838, 354.297241, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.482031, 0.00240341, 0.87615, 0.149719, -0.87612, -0.0100773, -0.481987, -0.000995038, 0.00767093, -0.999947, 0.00696306, 0.0062866, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.455658, 0.272167, 0.000796, -0.000206, -0.0975 ], "calibration_timestamp": 1358499769498335208, "camera_id": "23-02" }
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-08.json b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-08.json
new file mode 100644
index 0000000..8728938
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-08.json
@@ -0,0 +1 @@
+{ "node_name": "pi2", "team_number": 7971, "intrinsics": [ 888.573181, 0.0, 649.254517, 0.0, 888.493774, 370.136658, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.482031, 0.00240341, 0.87615, 0.149719, -0.87612, -0.0100773, -0.481987, -0.000995038, 0.00767093, -0.999947, 0.00696306, 0.0062866, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.449155, 0.251979, -0.000308, -0.000179, -0.080278 ], "calibration_timestamp": 1358499769498335208, "camera_id": "23-02" }
diff --git a/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-02-19.json b/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-02-19.json
deleted file mode 100644
index e2a5817..0000000
--- a/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-02-19.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi3", "team_number": 7971, "intrinsics": [ 892.089172, 0.0, 648.780701, 0.0, 892.362854, 342.340668, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.496784, 0.01435, 0.867756, 0.146101, -0.867856, 0.00168, 0.496813, 0.028557, 0.005671, -0.999896, 0.013288, -0.01621, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.451178, 0.258187, 0.001071, 0.000017, -0.085526 ], "calibration_timestamp": 1358501967378322133, "camera_id": "23-03" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-05.json b/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-05.json
new file mode 100644
index 0000000..f7b554c
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-05.json
@@ -0,0 +1 @@
+{ "node_name": "pi3", "team_number": 7971, "intrinsics": [ 892.089172, 0.0, 648.780701, 0.0, 892.362854, 342.340668, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.517483, 0.00906969, 0.855645, 0.156941, -0.855687, 0.00935422, 0.517409, 0.0833421, -0.00331132, -0.999915, 0.0126013, 0.0124048, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.451178, 0.258187, 0.001071, 0.000017, -0.085526 ], "calibration_timestamp": 1358501967378322133, "camera_id": "23-03" }
diff --git a/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-08.json b/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-08.json
new file mode 100644
index 0000000..6eb481f
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-08.json
@@ -0,0 +1 @@
+{ "node_name": "pi3", "team_number": 7971, "intrinsics": [ 887.863831, 0.0, 625.956909, 0.0, 888.210205, 372.510437, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.517483, 0.00906969, 0.855645, 0.156941, -0.855687, 0.00935422, 0.517409, 0.0833421, -0.00331132, -0.999915, 0.0126013, 0.0124048, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.449028, 0.257065, 0.000478, -0.000113, -0.086986 ], "calibration_timestamp": 1358501967378322133, "camera_id": "23-03" }
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-04_ext_2023-04-10.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-04_ext_2023-04-10.json
new file mode 100644
index 0000000..f76623c
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-04_ext_2023-04-10.json
@@ -0,0 +1,2 @@
+{ "node_name": "pi3", "team_number": 971, "intrinsics": [ 890.071899, 0.0, 620.69519, 0.0, 890.307434, 365.158844, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.482409, -0.181339, -0.85697, -0.115455, -0.871457, -0.000440151, -0.490471, -0.175596, 0.0885639, 0.983421, -0.158241, 0.61873, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.449088, 0.25594, 0.000415, 0.000142, -0.084656 ], "calibration_timestamp": 1358501982039874176, "camera_id": "23-04" }
+
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-02-22.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-02-22.json
deleted file mode 100644
index 8c86d20..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-02-22.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi3", "team_number": 971, "intrinsics": [ 892.627869, 0.0, 629.289978, 0.0, 891.73761, 373.299896, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.492232, -0.163335, -0.855002, 0.020122, -0.866067, 0.006706, -0.499883, -0.174518, 0.087382, 0.986548, -0.138158, 0.645307, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.454901, 0.266778, -0.000316, -0.000469, -0.091357 ], "calibration_timestamp": 1358500316768663953, "camera_id": "23-07" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-03-26.json b/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-03-26.json
deleted file mode 100644
index 64d1c95..0000000
--- a/y2023/vision/calib_files/calibration_pi-971-3_cam-23-07_ext_2023-03-26.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi3", "team_number": 971, "intrinsics": [ 892.627869, 0.0, 629.289978, 0.0, 891.73761, 373.299896, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [0.514403, -0.183311, -0.837728, -0.0051253, -0.851817, 0.00353495, -0.523827, -0.214622, 0.0989849, 0.983049, -0.154329, 0.62819, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.454901, 0.266778, -0.000316, -0.000469, -0.091357 ], "calibration_timestamp": 1358500316768663953, "camera_id": "23-07" }
diff --git a/y2023/vision/camera_monitor.cc b/y2023/vision/camera_monitor.cc
new file mode 100644
index 0000000..e69fd7c
--- /dev/null
+++ b/y2023/vision/camera_monitor.cc
@@ -0,0 +1,18 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2023/vision/camera_monitor_lib.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ y2023::vision::CameraMonitor monitor(&event_loop);
+
+ event_loop.Run();
+}
diff --git a/y2023/vision/camera_monitor_lib.cc b/y2023/vision/camera_monitor_lib.cc
new file mode 100644
index 0000000..c4ec0dc
--- /dev/null
+++ b/y2023/vision/camera_monitor_lib.cc
@@ -0,0 +1,37 @@
+#include "y2023/vision/camera_monitor_lib.h"
+namespace y2023::vision {
+namespace {
+// This needs to include the amount of time that it will take for the
+// camera_reader to start.
+constexpr std::chrono::seconds kImageTimeout{5};
+} // namespace
+CameraMonitor::CameraMonitor(aos::EventLoop *event_loop)
+ : event_loop_(event_loop), starter_(event_loop_) {
+ event_loop_->MakeNoArgWatcher<frc971::vision::CameraImage>(
+ "/camera", [this]() { SetImageTimeout(); });
+ starter_.SetTimeoutHandler([this]() {
+ LOG(WARNING) << "Failed to restart camera_reader when images timed out.";
+ SetImageTimeout();
+ });
+ starter_.SetSuccessHandler([this]() {
+ LOG(INFO) << "Finished restarting camera_reader.";
+ SetImageTimeout();
+ });
+
+ image_timeout_ = event_loop_->AddTimer([this]() {
+ LOG(INFO) << "Restarting camera_reader due to stale images.";
+ starter_.SendCommands({{aos::starter::Command::RESTART,
+ "camera_reader",
+ {event_loop_->node()}}},
+ /*timeout=*/std::chrono::seconds(3));
+ });
+ // If for some reason camera_reader fails to start up at all, we want to
+ // end up restarting things.
+ event_loop_->OnRun([this]() { SetImageTimeout(); });
+}
+
+void CameraMonitor::SetImageTimeout() {
+ image_timeout_->Setup(event_loop_->context().monotonic_event_time +
+ kImageTimeout);
+}
+} // namespace y2023::vision
diff --git a/y2023/vision/camera_monitor_lib.h b/y2023/vision/camera_monitor_lib.h
new file mode 100644
index 0000000..ddb116e
--- /dev/null
+++ b/y2023/vision/camera_monitor_lib.h
@@ -0,0 +1,21 @@
+#ifndef Y2023_VISION_CAMERA_MONITOR_LIB_H_
+#define Y2023_VISION_CAMERA_MONITOR_LIB_H_
+#include "aos/events/event_loop.h"
+#include "aos/starter/starter_rpc_lib.h"
+#include "frc971/vision/vision_generated.h"
+namespace y2023::vision {
+// This class provides an application that will restart the camera_reader
+// process whenever images stop flowing for too long. This is to mitigate an
+// issue where sometimes we stop getting camera images.
+class CameraMonitor {
+ public:
+ CameraMonitor(aos::EventLoop *event_loop);
+
+ private:
+ void SetImageTimeout();
+ aos::EventLoop *event_loop_;
+ aos::starter::StarterClient starter_;
+ aos::TimerHandler *image_timeout_;
+};
+} // namespace y2023::vision
+#endif // Y2023_VISION_CAMERA_MONITOR_LIB_H_
diff --git a/y2023/vision/game_pieces.cc b/y2023/vision/game_pieces.cc
index 0e2546f..8a0621d 100644
--- a/y2023/vision/game_pieces.cc
+++ b/y2023/vision/game_pieces.cc
@@ -5,95 +5,73 @@
#include "aos/events/event_loop.h"
#include "aos/events/shm_event_loop.h"
#include "frc971/vision/vision_generated.h"
-
-// The best_x and best_y are pixel (x, y) cordinates. The 'best'
-// game piece is picked on proximity to the specified cordinates.
-// The cordinate should represent where we want to intake a game piece.
-// (0, 360) was chosen without any testing, just a cordinate that
-// seemed reasonable.
-
-DEFINE_uint32(
- best_x, 0,
- "The 'best' game piece is picked based on how close it is to this x value");
-
-DEFINE_uint32(
- best_y, 360,
- "The 'best' game piece is picked based on how close it is to this y value");
+#include "y2023/vision/yolov5.h"
namespace y2023 {
namespace vision {
+using aos::monotonic_clock;
GamePiecesDetector::GamePiecesDetector(aos::EventLoop *event_loop)
: game_pieces_sender_(event_loop->MakeSender<GamePieces>("/camera")) {
- event_loop->MakeWatcher("/camera", [this](const CameraImage &camera_image) {
- this->ProcessImage(camera_image);
- });
+ model = MakeYOLOV5();
+ model->LoadModel("edgetpu_model.tflite");
+
+ event_loop->MakeWatcher(
+ "/camera", [this, event_loop](const CameraImage &camera_image) {
+ const monotonic_clock::time_point eof = monotonic_clock::time_point(
+ std::chrono::nanoseconds(camera_image.monotonic_timestamp_ns()));
+ const monotonic_clock::duration age = event_loop->monotonic_now() - eof;
+ if (age > std::chrono::milliseconds(100)) {
+ VLOG(1) << "Behind, skipping";
+ return;
+ }
+
+ this->ProcessImage(camera_image);
+ });
}
-// TODO(FILIP): Actually do inference.
-
void GamePiecesDetector::ProcessImage(const CameraImage &image) {
- // Param is not used for now.
- (void)image;
+ cv::Mat image_color_mat(cv::Size(image.cols(), image.rows()), CV_8UC2,
+ (void *)image.data()->data());
+ std::vector<Detection> detections;
+ cv::Mat image_mat(cv::Size(image.cols(), image.rows()), CV_8UC3);
+ cv::cvtColor(image_color_mat, image_mat, cv::COLOR_YUV2BGR_YUYV);
- const int detection_count = 5;
+ detections = model->ProcessImage(image_mat);
auto builder = game_pieces_sender_.MakeBuilder();
std::vector<flatbuffers::Offset<GamePiece>> game_pieces_offsets;
- float lowest_distance = std::numeric_limits<float>::max();
- int best_distance_index = 0;
- srand(time(0));
-
- for (int i = 0; i < detection_count; i++) {
- int h = rand() % 1000;
- int w = rand() % 1000;
- int x = rand() % 250;
- int y = rand() % 250;
-
+ for (size_t i = 0; i < detections.size(); i++) {
auto box_builder = builder.MakeBuilder<Box>();
- box_builder.add_h(h);
- box_builder.add_w(w);
- box_builder.add_x(x);
- box_builder.add_y(y);
+ box_builder.add_h(detections[i].box.height);
+ box_builder.add_w(detections[i].box.width);
+ box_builder.add_x(detections[i].box.x);
+ box_builder.add_y(detections[i].box.y);
auto box_offset = box_builder.Finish();
auto game_piece_builder = builder.MakeBuilder<GamePiece>();
- game_piece_builder.add_piece_class(y2023::vision::Class::CONE_DOWN);
+ switch (detections[i].class_id) {
+ case 0:
+ game_piece_builder.add_piece_class(ConeClass::CONE);
+ break;
+ default:
+ // Should never happen.
+ game_piece_builder.add_piece_class(ConeClass::NONE);
+ }
game_piece_builder.add_box(box_offset);
- game_piece_builder.add_confidence(0.9);
+ game_piece_builder.add_confidence(detections[i].confidence);
auto game_piece = game_piece_builder.Finish();
game_pieces_offsets.push_back(game_piece);
-
- // Center x and y values.
- // Inference returns the top left corner of the bounding box
- // but we want the center of the box for this.
-
- const int center_x = x + w / 2;
- const int center_y = y + h / 2;
-
- // Find difference between target x, y and the x, y
- // of the bounding box using Euclidean distance.
-
- const int dx = FLAGS_best_x - center_x;
- const int dy = FLAGS_best_y - center_y;
- const float distance = std::sqrt(dx * dx + dy * dy);
-
- if (distance < lowest_distance) {
- lowest_distance = distance;
- best_distance_index = i;
- }
};
- flatbuffers::FlatBufferBuilder fbb;
- auto game_pieces_vector = fbb.CreateVector(game_pieces_offsets);
+ auto game_pieces_vector = builder.fbb()->CreateVector(game_pieces_offsets);
auto game_pieces_builder = builder.MakeBuilder<GamePieces>();
game_pieces_builder.add_game_pieces(game_pieces_vector);
- game_pieces_builder.add_best_piece(best_distance_index);
builder.CheckOk(builder.Send(game_pieces_builder.Finish()));
}
} // namespace vision
-} // namespace y2023
\ No newline at end of file
+} // namespace y2023
diff --git a/y2023/vision/game_pieces.fbs b/y2023/vision/game_pieces.fbs
index 89f7505..1792ece 100644
--- a/y2023/vision/game_pieces.fbs
+++ b/y2023/vision/game_pieces.fbs
@@ -1,6 +1,6 @@
namespace y2023.vision;
-// Object class.
+// Not used by ml anymore.
enum Class : byte {
NONE = 0,
CONE_UP = 1,
@@ -8,6 +8,11 @@
CONE_DOWN = 3,
}
+enum ConeClass : byte {
+ NONE = 0,
+ CONE = 1,
+}
+
// Bounding box dimensions and position.
// X and Y represent the top left of the bounding box.
table Box {
@@ -18,14 +23,13 @@
}
table GamePiece {
- piece_class:Class (id: 0);
+ piece_class:ConeClass (id: 0);
box:Box (id:1);
confidence:float (id:2);
}
table GamePieces {
game_pieces:[GamePiece] (id: 0);
- best_piece:uint (id: 1); // Index of the "best piece".
}
root_type GamePieces;
diff --git a/y2023/vision/game_pieces.h b/y2023/vision/game_pieces.h
index a41d52a..a99a99b 100644
--- a/y2023/vision/game_pieces.h
+++ b/y2023/vision/game_pieces.h
@@ -5,6 +5,8 @@
#include "frc971/vision/vision_generated.h"
#include "y2023/vision/game_pieces_generated.h"
+#include "y2023/vision/yolov5.h"
+
namespace y2023 {
namespace vision {
@@ -20,6 +22,7 @@
private:
aos::Sender<GamePieces> game_pieces_sender_;
+ std::unique_ptr<YOLOV5> model;
};
} // namespace vision
} // namespace y2023
diff --git a/y2023/vision/game_pieces_detector_starter.sh b/y2023/vision/game_pieces_detector_starter.sh
new file mode 100755
index 0000000..36dca97
--- /dev/null
+++ b/y2023/vision/game_pieces_detector_starter.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/home/pi/bin game_pieces_detector
diff --git a/y2023/vision/image_logger.cc b/y2023/vision/image_logger.cc
index d90f9c2..29a95db 100644
--- a/y2023/vision/image_logger.cc
+++ b/y2023/vision/image_logger.cc
@@ -18,10 +18,17 @@
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>(
- absl::StrCat(aos::logging::GetLogName("fbs_log"), "/"), event_loop);
+ std::optional<std::string> log_name =
+ aos::logging::MaybeGetLogName("fbs_log");
+
+ if (!log_name.has_value()) {
+ return nullptr;
+ }
+
+ return std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
+ absl::StrCat(log_name.value(), "/"), event_loop);
}
int main(int argc, char *argv[]) {
@@ -71,9 +78,14 @@
enabled = joystick_state.enabled();
if (!logging && enabled) {
+ auto log_namer = MakeLogNamer(&event_loop);
+ if (log_namer == nullptr) {
+ return;
+ }
+
// Start logging if we just got enabled
LOG(INFO) << "Starting logging";
- logger.StartLogging(MakeLogNamer(&event_loop));
+ logger.StartLogging(std::move(log_namer));
logging = true;
last_rotation_time = event_loop.monotonic_now();
} else if (logging && !enabled &&
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index e9ef662..daba90c 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -5,7 +5,6 @@
#include "aos/util/mcap_logger.h"
#include "frc971/control_loops/pose.h"
#include "frc971/vision/calibration_generated.h"
-#include "frc971/vision/charuco_lib.h"
#include "frc971/vision/target_mapper.h"
#include "frc971/vision/visualize_robot.h"
#include "opencv2/aruco.hpp"
@@ -41,9 +40,18 @@
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.");
+DEFINE_string(dump_constraints_to, "/tmp/constraints.txt",
+ "Write the target constraints to this path");
+DECLARE_int32(frozen_target_id);
+DECLARE_int32(min_target_id);
+DECLARE_int32(max_target_id);
+DECLARE_bool(visualize_solver);
namespace y2023 {
namespace vision {
@@ -55,47 +63,188 @@
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_solver) {
+ 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) {
+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 << " - ";
for (const auto *target_pose_fbs : *map.target_poses()) {
+ // Skip detections with invalid ids
+ if (static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) <
+ FLAGS_min_target_id ||
+ static_cast<TargetMapper::TargetId>(target_pose_fbs->id()) >
+ FLAGS_max_target_id) {
+ VLOG(1) << "Skipping tag with invalid id of " << target_pose_fbs->id();
+ continue;
+ }
+
// Skip detections with high pose errors
if (target_pose_fbs->pose_error() > FLAGS_max_pose_error) {
- VLOG(1) << " Skipping tag " << target_pose_fbs->id()
+ VLOG(1) << "Skipping tag " << target_pose_fbs->id()
<< " due to pose error of " << target_pose_fbs->pose_error();
continue;
}
// Skip detections with high pose error ratios
if (target_pose_fbs->pose_error_ratio() > FLAGS_max_pose_error_ratio) {
- VLOG(1) << " Skipping tag " << target_pose_fbs->id()
+ VLOG(1) << "Skipping tag " << target_pose_fbs->id()
<< " due to pose error ratio of "
<< target_pose_fbs->pose_error_ratio();
continue;
@@ -117,7 +266,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,
@@ -125,28 +274,31 @@
.distortion_factor = distortion_factor,
.id = static_cast<TargetMapper::TargetId>(target_pose.id)});
- if (FLAGS_visualize) {
+ if (FLAGS_visualize_solver) {
// 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
@@ -159,36 +311,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_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);
+ drawn_nodes_.emplace(node_name);
+ } else if (pi_distributed_time - last_draw_time_ >
+ std::chrono::milliseconds(30) &&
+ 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_);
+ 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) {
+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
@@ -201,26 +363,90 @@
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);
+ HandleAprilTags(map, pi_distributed_time, node_name, extrinsics);
});
}
+void TargetMapperReplay::MaybeSolve() {
+ if (FLAGS_solve) {
+ auto target_constraints =
+ DataAdapter::MatchTargetDetections(timestamped_target_detections_);
+
+ // Remove constraints between the two sides of the field - these are
+ // basically garbage because of how far the camera is. We will use seeding
+ // below to connect the two sides
+ target_constraints.erase(
+ std::remove_if(target_constraints.begin(), target_constraints.end(),
+ [](const auto &constraint) {
+ constexpr TargetMapper::TargetId kMaxRedId = 4;
+ TargetMapper::TargetId min_id =
+ std::min(constraint.id_begin, constraint.id_end);
+ TargetMapper::TargetId max_id =
+ std::max(constraint.id_begin, constraint.id_end);
+ return (min_id <= kMaxRedId && max_id > kMaxRedId);
+ }),
+ target_constraints.end());
+
+ if (!FLAGS_dump_constraints_to.empty()) {
+ std::ofstream fout(FLAGS_dump_constraints_to);
+ for (const auto &constraint : target_constraints) {
+ fout << constraint << std::endl;
+ }
+ fout.flush();
+ fout.close();
+ }
+
+ // Give seed constraints with a higher confidence to ground the solver.
+ // This "distance from camera" controls the noise of the seed measurement
+ constexpr double kSeedDistanceFromCamera = 1.0;
+
+ constexpr double kSeedDistortionFactor = 0.0;
+ const DataAdapter::TimestampedDetection frozen_detection_seed = {
+ .time = aos::distributed_clock::min_time,
+ .H_robot_target = PoseUtils::Pose3dToAffine3d(
+ kFixedTargetMapper.GetTargetPoseById(FLAGS_frozen_target_id)
+ .value()
+ .pose),
+ .distance_from_camera = kSeedDistanceFromCamera,
+ .distortion_factor = kSeedDistortionFactor,
+ .id = FLAGS_frozen_target_id};
+
+ constexpr TargetMapper::TargetId kAbsMinTargetId = 1;
+ constexpr TargetMapper::TargetId kAbsMaxTargetId = 8;
+ for (TargetMapper::TargetId id = kAbsMinTargetId; id <= kAbsMaxTargetId;
+ id++) {
+ if (id == FLAGS_frozen_target_id) {
+ continue;
+ }
+
+ const DataAdapter::TimestampedDetection detection_seed = {
+ .time = aos::distributed_clock::min_time,
+ .H_robot_target = PoseUtils::Pose3dToAffine3d(
+ kFixedTargetMapper.GetTargetPoseById(id).value().pose),
+ .distance_from_camera = kSeedDistanceFromCamera,
+ .distortion_factor = kSeedDistortionFactor,
+ .id = id};
+ target_constraints.emplace_back(DataAdapter::ComputeTargetConstraint(
+ frozen_detection_seed, detection_seed,
+ DataAdapter::ComputeConfidence(frozen_detection_seed,
+ detection_seed)));
+ }
+
+ 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);
@@ -232,113 +458,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;
-
- 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);
-
- 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);
-
- 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);
-
- 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);
-
- 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/vision/yolov5.cc b/y2023/vision/yolov5.cc
new file mode 100644
index 0000000..473437c
--- /dev/null
+++ b/y2023/vision/yolov5.cc
@@ -0,0 +1,302 @@
+#include "yolov5.h"
+
+#include <tensorflow/lite/c/common.h>
+#include <tensorflow/lite/interpreter.h>
+#include <tensorflow/lite/kernels/register.h>
+#include <tensorflow/lite/model.h>
+#include <tflite/public/edgetpu.h>
+#include <tflite/public/edgetpu_c.h>
+
+#include <chrono>
+#include <opencv2/dnn.hpp>
+#include <string>
+
+#include "absl/types/span.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+DEFINE_double(conf_threshold, 0.9,
+ "Threshold value for confidence scores. Detections with a "
+ "confidence score below this value will be ignored.");
+
+DEFINE_double(
+ nms_threshold, 0.5,
+ "Threshold value for non-maximum suppression. Detections with an "
+ "intersection-over-union value below this value will be removed.");
+
+DEFINE_int32(nthreads, 6, "Number of threads to use during inference.");
+
+DEFINE_bool(visualize_detections, false, "Display inference output");
+
+namespace y2023 {
+namespace vision {
+
+class YOLOV5Impl : public YOLOV5 {
+ public:
+ // Takes a model path as string and and loads a pre-trained
+ // YOLOv5 model from the specified path.
+ void LoadModel(const std::string path);
+
+ // Takes an image and returns a Detection.
+ std::vector<Detection> ProcessImage(cv::Mat image);
+
+ private:
+ // Convert an OpenCV Mat object to a tensor input
+ // that can be fed to the TensorFlow Lite model.
+ void ConvertCVMatToTensor(cv::Mat src, absl::Span<uint8_t> tensor);
+
+ // Resizes, converts color space, and converts
+ // image data type before inference.
+ void Preprocess(cv::Mat image);
+
+ // Converts a TensorFlow Lite tensor to a 2D vector.
+ std::vector<std::vector<float>> TensorToVector2D(TfLiteTensor *src_tensor,
+ const int rows,
+ const int columns);
+
+ // Performs non-maximum suppression to remove overlapping bounding boxes.
+ std::vector<Detection> NonMaximumSupression(
+ const std::vector<std::vector<float>> &orig_preds, const int rows,
+ const int columns, std::vector<Detection> *detections,
+ std::vector<int> *indices);
+ // Models
+ std::unique_ptr<tflite::FlatBufferModel> model_;
+ std::unique_ptr<tflite::Interpreter> interpreter_;
+ tflite::StderrReporter error_reporter_;
+
+ // Parameters of interpreter's input
+ int input_;
+ int in_height_;
+ int in_width_;
+ int in_channels_;
+ int in_type_;
+
+ // Parameters of original image
+ int img_height_;
+ int img_width_;
+
+ // Input of the interpreter
+ absl::Span<uint8_t> input_8_;
+
+ // Subtract this offset from class labels to get the actual label.
+ static constexpr int kClassIdOffset = 5;
+};
+
+std::unique_ptr<YOLOV5> MakeYOLOV5() { return std::make_unique<YOLOV5Impl>(); }
+
+void YOLOV5Impl::LoadModel(const std::string path) {
+ VLOG(1) << "Load model: Start";
+
+ tflite::ops::builtin::BuiltinOpResolver resolver;
+
+ model_ = tflite::FlatBufferModel::VerifyAndBuildFromFile(path.c_str());
+ CHECK(model_);
+ CHECK(model_->initialized());
+ VLOG(1) << "Load model: Build model from file success";
+
+ CHECK_EQ(tflite::InterpreterBuilder(*model_, resolver)(&interpreter_),
+ kTfLiteOk);
+ VLOG(1) << "Load model: Interpreter builder success";
+
+ size_t num_devices;
+ std::unique_ptr<edgetpu_device, decltype(&edgetpu_free_devices)> devices(
+ edgetpu_list_devices(&num_devices), &edgetpu_free_devices);
+
+ CHECK_EQ(num_devices, 1ul);
+ const auto &device = devices.get()[0];
+ VLOG(1) << "Load model: Got Devices";
+
+ auto *delegate =
+ edgetpu_create_delegate(device.type, device.path, nullptr, 0);
+
+ interpreter_->ModifyGraphWithDelegate(delegate);
+
+ VLOG(1) << "Load model: Modify graph with delegate complete";
+
+ TfLiteStatus status = interpreter_->AllocateTensors();
+ CHECK_EQ(status, kTfLiteOk);
+ CHECK(interpreter_);
+
+ VLOG(1) << "Load model: Allocate tensors success";
+
+ input_ = interpreter_->inputs()[0];
+ TfLiteIntArray *dims = interpreter_->tensor(input_)->dims;
+ in_height_ = dims->data[1];
+ in_width_ = dims->data[2];
+ in_channels_ = dims->data[3];
+ in_type_ = interpreter_->tensor(input_)->type;
+
+ int tensor_size = 1;
+ for (int i = 0; i < dims->size; i++) {
+ tensor_size *= dims->data[i];
+ }
+ input_8_ =
+ absl::Span(interpreter_->typed_tensor<uint8_t>(input_), tensor_size);
+
+ interpreter_->SetNumThreads(FLAGS_nthreads);
+
+ VLOG(1) << "Load model: Done";
+}
+
+void YOLOV5Impl::ConvertCVMatToTensor(cv::Mat src, absl::Span<uint8_t> tensor) {
+ CHECK(src.type() == CV_8UC3);
+ int n = 0, nc = src.channels(), ne = src.elemSize();
+ VLOG(2) << "ConvertCVMatToTensor: Rows " << src.rows;
+ VLOG(2) << "ConvertCVMatToTensor: Cols " << src.cols;
+ for (int y = 0; y < src.rows; ++y) {
+ auto *row_ptr = src.ptr<uint8_t>(y);
+ for (int x = 0; x < src.cols; ++x) {
+ for (int c = 0; c < nc; ++c) {
+ tensor[n++] = *(row_ptr + x * ne + c);
+ }
+ }
+ }
+}
+
+std::vector<std::vector<float>> YOLOV5Impl::TensorToVector2D(
+ TfLiteTensor *src_tensor, const int rows, const int columns) {
+ auto scale = src_tensor->params.scale;
+ auto zero_point = src_tensor->params.zero_point;
+ std::vector<std::vector<float>> result_vec;
+ for (int32_t i = 0; i < rows; i++) {
+ std::vector<float> row_values;
+ for (int32_t j = 0; j < columns; j++) {
+ float val_float =
+ ((static_cast<int32_t>(src_tensor->data.uint8[i * columns + j])) -
+ zero_point) *
+ scale;
+ row_values.push_back(val_float);
+ }
+ result_vec.push_back(row_values);
+ }
+ return result_vec;
+}
+
+std::vector<Detection> YOLOV5Impl::NonMaximumSupression(
+ const std::vector<std::vector<float>> &orig_preds, const int rows,
+ const int columns, std::vector<Detection> *detections,
+ std::vector<int> *indices)
+
+{
+ std::vector<float> scores;
+ double confidence;
+ cv::Point class_id;
+
+ for (int i = 0; i < rows; i++) {
+ if (orig_preds[i][4] > FLAGS_conf_threshold) {
+ float x = orig_preds[i][0];
+ float y = orig_preds[i][1];
+ float w = orig_preds[i][2];
+ float h = orig_preds[i][3];
+ int left = static_cast<int>((x - 0.5 * w) * img_width_);
+ int top = static_cast<int>((y - 0.5 * h) * img_height_);
+ int width = static_cast<int>(w * img_width_);
+ int height = static_cast<int>(h * img_height_);
+
+ for (int j = 5; j < columns; j++) {
+ scores.push_back(orig_preds[i][j] * orig_preds[i][4]);
+ }
+
+ cv::minMaxLoc(scores, nullptr, &confidence, nullptr, &class_id);
+ scores.clear();
+ if (confidence > FLAGS_conf_threshold) {
+ Detection detection{cv::Rect(left, top, width, height), confidence,
+ class_id.x};
+ detections->push_back(detection);
+ }
+ }
+ }
+
+ std::vector<cv::Rect> boxes;
+ std::vector<float> confidences;
+
+ for (const Detection &d : *detections) {
+ boxes.push_back(d.box);
+ confidences.push_back(d.confidence);
+ }
+
+ cv::dnn::NMSBoxes(boxes, confidences, FLAGS_conf_threshold,
+ FLAGS_nms_threshold, *indices);
+
+ std::vector<Detection> filtered_detections;
+ for (size_t i = 0; i < indices->size(); i++) {
+ filtered_detections.push_back((*detections)[(*indices)[i]]);
+ }
+
+ VLOG(1) << "NonMaximumSupression: " << detections->size() - indices->size()
+ << " detections filtered out";
+
+ return filtered_detections;
+}
+
+std::vector<Detection> YOLOV5Impl::ProcessImage(cv::Mat frame) {
+ VLOG(1) << "\n";
+
+ auto start = std::chrono::high_resolution_clock::now();
+ img_height_ = frame.rows;
+ img_width_ = frame.cols;
+
+ cv::resize(frame, frame, cv::Size(in_height_, in_width_), cv::INTER_CUBIC);
+ cv::cvtColor(frame, frame, cv::COLOR_BGR2RGB);
+ frame.convertTo(frame, CV_8U);
+
+ ConvertCVMatToTensor(frame, input_8_);
+
+ TfLiteStatus status = interpreter_->Invoke();
+ CHECK_EQ(status, kTfLiteOk);
+
+ int output_tensor_index = interpreter_->outputs()[0];
+ TfLiteIntArray *out_dims = interpreter_->tensor(output_tensor_index)->dims;
+ int num_rows = out_dims->data[1];
+ int num_columns = out_dims->data[2];
+
+ TfLiteTensor *src_tensor = interpreter_->tensor(interpreter_->outputs()[0]);
+
+ std::vector<std::vector<float>> orig_preds =
+ TensorToVector2D(src_tensor, num_rows, num_columns);
+
+ std::vector<int> indices;
+ std::vector<Detection> detections;
+
+ std::vector<Detection> filtered_detections;
+ filtered_detections = NonMaximumSupression(orig_preds, num_rows, num_columns,
+ &detections, &indices);
+ VLOG(1) << "---";
+ for (size_t i = 0; i < filtered_detections.size(); i++) {
+ VLOG(1) << "Detection #" << i << " | Class ID #"
+ << filtered_detections[i].class_id << " @ "
+ << filtered_detections[i].confidence << " confidence";
+ }
+
+ VLOG(1) << "---";
+
+ auto stop = std::chrono::high_resolution_clock::now();
+
+ VLOG(1) << "Inference time: "
+ << std::chrono::duration_cast<std::chrono::milliseconds>(stop - start)
+ .count();
+
+ if (FLAGS_visualize_detections) {
+ cv::resize(frame, frame, cv::Size(img_width_, img_height_), 0, 0, true);
+ for (size_t i = 0; i < filtered_detections.size(); i++) {
+ VLOG(1) << "Bounding Box | X: " << filtered_detections[i].box.x
+ << " Y: " << filtered_detections[i].box.y
+ << " W: " << filtered_detections[i].box.width
+ << " H: " << filtered_detections[i].box.height;
+ cv::rectangle(frame, filtered_detections[i].box, cv::Scalar(255, 0, 0),
+ 2);
+ cv::putText(
+ frame, std::to_string(filtered_detections[i].class_id),
+ cv::Point(filtered_detections[i].box.x, filtered_detections[i].box.y),
+ cv::FONT_HERSHEY_COMPLEX, 1.0, cv::Scalar(0, 0, 255), 1, cv::LINE_AA);
+ }
+ cv::cvtColor(frame, frame, cv::COLOR_BGR2RGB);
+ cv::imshow("yolo", frame);
+ cv::waitKey(10);
+ }
+
+ return filtered_detections;
+};
+
+} // namespace vision
+} // namespace y2023
diff --git a/y2023/vision/yolov5.h b/y2023/vision/yolov5.h
new file mode 100644
index 0000000..7e2a521
--- /dev/null
+++ b/y2023/vision/yolov5.h
@@ -0,0 +1,41 @@
+#ifndef Y2023_VISION_YOLOV5_H_
+#define Y2023_VISION_YOLOV5_H_
+
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <fstream>
+#include <iostream>
+#include <opencv2/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+#include <vector>
+
+namespace y2023 {
+namespace vision {
+
+struct Detection {
+ cv::Rect box;
+ double confidence;
+ int class_id;
+};
+
+class YOLOV5 {
+ public:
+ virtual ~YOLOV5() {}
+
+ // Takes a model path as string and loads a pre-trained
+ // YOLOv5 model from the specified path.
+ virtual void LoadModel(const std::string path) = 0;
+
+ // Takes an image and returns a Detection.
+ virtual std::vector<Detection> ProcessImage(cv::Mat image) = 0;
+};
+
+std::unique_ptr<YOLOV5> MakeYOLOV5();
+
+} // namespace vision
+} // namespace y2023
+
+#endif // Y2023_VISION_YOLOV5_H_
diff --git a/y2023/www/field.html b/y2023/www/field.html
index cc89bb9..6bd2fc0 100644
--- a/y2023/www/field.html
+++ b/y2023/www/field.html
@@ -56,6 +56,10 @@
<td>Game Piece Held</td>
<td id="game_piece"> NA </td>
</tr>
+ <tr>
+ <td>Game Piece Position (+ = left, 0 = empty)</td>
+ <td id="game_piece_position"> NA </td>
+ </tr>
</table>
<table>
diff --git a/y2023/www/field_handler.ts b/y2023/www/field_handler.ts
index 2f62c7d..24a55fa 100644
--- a/y2023/www/field_handler.ts
+++ b/y2023/www/field_handler.ts
@@ -56,6 +56,8 @@
(document.getElementById('arm_state') as HTMLElement);
private gamePiece: HTMLElement =
(document.getElementById('game_piece') as HTMLElement);
+ private gamePiecePosition: HTMLElement =
+ (document.getElementById('game_piece_position') as HTMLElement);
private armX: HTMLElement = (document.getElementById('arm_x') as HTMLElement);
private armY: HTMLElement = (document.getElementById('arm_y') as HTMLElement);
private circularIndex: HTMLElement =
@@ -387,6 +389,8 @@
this.armState.innerHTML =
ArmState[this.superstructureStatus.arm().state()];
this.gamePiece.innerHTML = Class[this.superstructureStatus.gamePiece()];
+ this.gamePiecePosition.innerHTML =
+ this.superstructureStatus.gamePiecePosition().toFixed(4);
this.armX.innerHTML = this.superstructureStatus.arm().armX().toFixed(2);
this.armY.innerHTML = this.superstructureStatus.arm().armY().toFixed(2);
this.circularIndex.innerHTML =
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index df3a55b..8c7f245 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -407,8 +407,7 @@
"user": "pi",
"args": [
"--enable_ftrace",
- "--send_downsized_images",
- "--exposure=650"
+ "--send_downsized_images"
],
"nodes": [
"logger"
@@ -448,8 +447,18 @@
]
},
{
- "name": "game_piece_detector",
- "executable_name": "game_piece_detector",
+ "name": "camera_monitor",
+ "executable_name": "camera_monitor",
+ "user": "pi",
+ "nodes": [
+ "logger"
+ ]
+ },
+ {
+ "name": "game_pieces_detector_starter",
+ "executable_name": "game_pieces_detector_starter.sh",
+ "autostart": true,
+ "user": "pi",
"nodes": [
"logger"
]
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index 46678f3..8167ba0 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -384,6 +384,14 @@
]
},
{
+ "name": "camera_monitor",
+ "executable_name": "camera_monitor",
+ "user": "pi",
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
+ },
+ {
"name": "foxglove_websocket",
"user": "pi",
"nodes": [
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"
]