Merge "Send downsized images for streaming"
diff --git a/aos/containers/resizeable_buffer.h b/aos/containers/resizeable_buffer.h
index 4a4038c..ed0d23e 100644
--- a/aos/containers/resizeable_buffer.h
+++ b/aos/containers/resizeable_buffer.h
@@ -11,18 +11,21 @@
// Kind of like a subset of vector<uint8_t>, but with less destructor calls.
// When building unoptimized, especially with sanitizers, the vector<uint8_t>
// version ends up being really slow in tests.
-class ResizeableBuffer {
+//
+// F is the allocator used for reallocating the memory used by the buffer.
+template <class F>
+class AllocatorResizeableBuffer {
public:
- ResizeableBuffer() = default;
+ AllocatorResizeableBuffer() = default;
- ResizeableBuffer(const ResizeableBuffer &other) { *this = other; }
- ResizeableBuffer(ResizeableBuffer &&other) { *this = std::move(other); }
- ResizeableBuffer &operator=(const ResizeableBuffer &other) {
+ AllocatorResizeableBuffer(const AllocatorResizeableBuffer &other) { *this = other; }
+ AllocatorResizeableBuffer(AllocatorResizeableBuffer &&other) { *this = std::move(other); }
+ AllocatorResizeableBuffer &operator=(const AllocatorResizeableBuffer &other) {
resize(other.size());
memcpy(storage_.get(), other.storage_.get(), size());
return *this;
}
- ResizeableBuffer &operator=(ResizeableBuffer &&other) {
+ AllocatorResizeableBuffer &operator=(AllocatorResizeableBuffer &&other) {
std::swap(storage_, other.storage_);
std::swap(size_, other.size_);
std::swap(capacity_, other.capacity_);
@@ -93,7 +96,7 @@
void Allocate(size_t new_capacity) {
void *const old = storage_.release();
- storage_.reset(CHECK_NOTNULL(realloc(old, new_capacity)));
+ storage_.reset(CHECK_NOTNULL(F::Realloc(old, capacity_, new_capacity)));
capacity_ = new_capacity;
}
@@ -101,6 +104,18 @@
size_t size_ = 0, capacity_ = 0;
};
+// An allocator which just uses realloc to allocate.
+class Reallocator {
+ public:
+ static void *Realloc(void *old, size_t /*old_size*/, size_t new_capacity) {
+ return realloc(old, new_capacity);
+ }
+};
+
+// A resizable buffer which uses realloc when it needs to grow to attempt to
+// avoid full coppies.
+class ResizeableBuffer : public AllocatorResizeableBuffer<Reallocator> {};
+
} // namespace aos
#endif // AOS_CONTAINERS_RESIZEABLE_BUFFER_H_
diff --git a/aos/events/logging/buffer_encoder.cc b/aos/events/logging/buffer_encoder.cc
index ae20440..5c1d4b2 100644
--- a/aos/events/logging/buffer_encoder.cc
+++ b/aos/events/logging/buffer_encoder.cc
@@ -9,34 +9,32 @@
namespace aos::logger {
-DummyEncoder::DummyEncoder(size_t max_buffer_size) {
- // TODO(austin): This is going to end up writing > 128k chunks, not 128k
- // chunks exactly. If we really want to, we could make it always write 128k
- // chunks by only exposing n * 128k chunks as we go. This might improve write
- // performance, then again, it might have no effect if the kernel is combining
- // writes...
- constexpr size_t kWritePageSize = 128 * 1024;
+DummyEncoder::DummyEncoder(size_t /*max_message_size*/, size_t buffer_size) {
// Round up to the nearest page size.
- input_buffer_.reserve(
- ((max_buffer_size + kWritePageSize - 1) / kWritePageSize) *
- kWritePageSize);
+ input_buffer_.reserve(buffer_size);
return_queue_.resize(1);
}
-bool DummyEncoder::HasSpace(size_t request) const {
- return request + input_buffer_.size() < input_buffer_.capacity();
+size_t DummyEncoder::space() const {
+ return input_buffer_.capacity() - input_buffer_.size();
}
-void DummyEncoder::Encode(Copier *copy) {
- DCHECK(HasSpace(copy->size()));
+bool DummyEncoder::HasSpace(size_t request) const { return request <= space(); }
+
+size_t DummyEncoder::Encode(Copier *copy, size_t start_byte) {
const size_t input_buffer_initial_size = input_buffer_.size();
- input_buffer_.resize(input_buffer_initial_size + copy->size());
+ size_t expected_write_size =
+ std::min(input_buffer_.capacity() - input_buffer_initial_size,
+ copy->size() - start_byte);
+ input_buffer_.resize(input_buffer_initial_size + expected_write_size);
const size_t written_size =
- copy->Copy(input_buffer_.data() + input_buffer_initial_size);
- DCHECK_EQ(written_size, copy->size());
+ copy->Copy(input_buffer_.data() + input_buffer_initial_size, start_byte,
+ expected_write_size + start_byte);
total_bytes_ += written_size;
+
+ return written_size;
}
void DummyEncoder::Clear(const int n) {
diff --git a/aos/events/logging/buffer_encoder.h b/aos/events/logging/buffer_encoder.h
index 4cd661d..ed5bef6 100644
--- a/aos/events/logging/buffer_encoder.h
+++ b/aos/events/logging/buffer_encoder.h
@@ -23,24 +23,28 @@
size_t size() const { return size_; }
// Writes size() bytes to data, and returns the data written.
- [[nodiscard]] virtual size_t Copy(uint8_t *data) = 0;
+ [[nodiscard]] virtual size_t Copy(uint8_t *data, size_t start_byte,
+ size_t end_byte) = 0;
private:
size_t size_;
};
- // Coppies a span. The span must have a longer lifetime than the coppier is
+ // Copies a span. The span must have a longer lifetime than the coppier is
// being used.
class SpanCopier : public Copier {
public:
SpanCopier(absl::Span<const uint8_t> data)
: Copier(data.size()), data_(data) {
- CHECK(data_.data());
+ CHECK(data_.data() != nullptr);
}
- size_t Copy(uint8_t *data) final {
- std::memcpy(data, data_.data(), data_.size());
- return data_.size();
+ size_t Copy(uint8_t *data, size_t start_byte, size_t end_byte) final {
+ DCHECK_LE(start_byte, end_byte);
+ DCHECK_LE(end_byte, data_.size());
+
+ std::memcpy(data, data_.data() + start_byte, end_byte - start_byte);
+ return end_byte - start_byte;
}
private:
@@ -51,12 +55,13 @@
// the output needs to be flushed.
virtual bool HasSpace(size_t request) const = 0;
- // Encodes and enqueues the given data encoder.
- virtual void Encode(Copier *copy) = 0;
+ // Returns the space available.
+ virtual size_t space() const = 0;
- // If this returns true, the encoder may be bypassed by writing directly to
- // the file.
- virtual bool may_bypass() const { return false; }
+ // Encodes and enqueues the given data encoder. Starts at the start byte
+ // (which must be a multiple of 8 bytes), and goes as far as it can. Returns
+ // the amount encoded.
+ virtual size_t Encode(Copier *copy, size_t start_byte) = 0;
// Finalizes the encoding process. After this, queue_size() represents the
// full extent of data which will be written to this file.
@@ -86,7 +91,7 @@
// and queues it up as is.
class DummyEncoder final : public DataEncoder {
public:
- DummyEncoder(size_t max_buffer_size);
+ DummyEncoder(size_t max_message_size, size_t buffer_size = 128 * 1024);
DummyEncoder(const DummyEncoder &) = delete;
DummyEncoder(DummyEncoder &&other) = delete;
DummyEncoder &operator=(const DummyEncoder &) = delete;
@@ -94,8 +99,8 @@
~DummyEncoder() override = default;
bool HasSpace(size_t request) const final;
- void Encode(Copier *copy) final;
- bool may_bypass() const final { return true; }
+ size_t space() const final;
+ size_t Encode(Copier *copy, size_t start_byte) final;
void Finish() final {}
void Clear(int n) final;
absl::Span<const absl::Span<const uint8_t>> queue() final;
@@ -108,7 +113,21 @@
private:
size_t total_bytes_ = 0;
- ResizeableBuffer input_buffer_;
+ // A class which uses aligned_alloc to allocate sector aligned blocks of
+ // memory.
+ 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> input_buffer_;
std::vector<absl::Span<const uint8_t>> return_queue_;
};
diff --git a/aos/events/logging/buffer_encoder_param_test.h b/aos/events/logging/buffer_encoder_param_test.h
index e6c4867..6085779 100644
--- a/aos/events/logging/buffer_encoder_param_test.h
+++ b/aos/events/logging/buffer_encoder_param_test.h
@@ -20,15 +20,12 @@
public:
static constexpr size_t kMaxMessageSize = 2 * 1024 * 1024;
- class DetachedBufferCopier : public DataEncoder::Copier {
+ class DetachedBufferCopier : public DataEncoder::SpanCopier {
public:
DetachedBufferCopier(flatbuffers::DetachedBuffer &&data)
- : DataEncoder::Copier(data.size()), data_(std::move(data)) {}
-
- size_t Copy(uint8_t *data) final {
- std::memcpy(data, data_.data(), data_.size());
- return data_.size();
- }
+ : DataEncoder::SpanCopier(
+ absl::Span<const uint8_t>(data.data(), data.size())),
+ data_(std::move(data)) {}
private:
const flatbuffers::DetachedBuffer data_;
@@ -48,7 +45,7 @@
<< encoder->queued_bytes() << ", encoding " << buffer.size();
DetachedBufferCopier coppier(std::move(buffer));
- encoder->Encode(&coppier);
+ encoder->Encode(&coppier, 0);
}
return result;
}
diff --git a/aos/events/logging/buffer_encoder_test.cc b/aos/events/logging/buffer_encoder_test.cc
index 8e12e37..d5e2e5d 100644
--- a/aos/events/logging/buffer_encoder_test.cc
+++ b/aos/events/logging/buffer_encoder_test.cc
@@ -13,9 +13,12 @@
class DummyEncoderTest : public BufferEncoderBaseTest {};
+constexpr size_t kEncoderBufferSize = 4 * 1024 * 1024;
+
// Tests that buffers are concatenated without being modified.
TEST_F(DummyEncoderTest, QueuesBuffersAsIs) {
- DummyEncoder encoder(BufferEncoderBaseTest::kMaxMessageSize);
+ DummyEncoder encoder(BufferEncoderBaseTest::kMaxMessageSize,
+ kEncoderBufferSize);
const auto expected = CreateAndEncode(100, &encoder);
std::vector<uint8_t> data = Flatten(expected);
@@ -26,7 +29,8 @@
// Tests that buffers are concatenated without being modified.
TEST_F(DummyEncoderTest, CoppiesBuffersAsIs) {
- DummyEncoder encoder(BufferEncoderBaseTest::kMaxMessageSize);
+ DummyEncoder encoder(BufferEncoderBaseTest::kMaxMessageSize,
+ kEncoderBufferSize);
const auto expected = CreateAndEncode(100, &encoder);
std::vector<uint8_t> data = Flatten(expected);
@@ -108,11 +112,41 @@
INSTANTIATE_TEST_SUITE_P(
Dummy, BufferEncoderTest,
::testing::Combine(::testing::Values([](size_t max_buffer_size) {
- return std::make_unique<DummyEncoder>(max_buffer_size);
+ return std::make_unique<DummyEncoder>(
+ max_buffer_size, kEncoderBufferSize);
}),
::testing::Values([](std::string_view filename) {
return std::make_unique<DummyDecoder>(filename);
}),
::testing::Range(0, 100)));
+// Tests that SpanCopier copies as expected.
+TEST(SpanCopierTest, Matches) {
+ std::vector<uint8_t> data;
+ for (int i = 0; i < 32; ++i) {
+ data.push_back(i);
+ }
+
+ CHECK_EQ(data.size(), 32u);
+
+ for (int i = 0; i < 32; i += 8) {
+ for (int j = i; j < 32; j += 8) {
+ std::vector<uint8_t> destination(data.size(), 0);
+ DataEncoder::SpanCopier copier(
+ absl::Span<const uint8_t>(data.data(), data.size()));
+
+ copier.Copy(destination.data(), i, j);
+
+ size_t index = 0;
+ for (int k = i; k < j; ++k) {
+ EXPECT_EQ(destination[index], k);
+ ++index;
+ }
+ for (; index < destination.size(); ++index) {
+ EXPECT_EQ(destination[index], 0u);
+ }
+ }
+ }
+}
+
} // namespace aos::logger::testing
diff --git a/aos/events/logging/log_namer.cc b/aos/events/logging/log_namer.cc
index c0c7c73..86d813f 100644
--- a/aos/events/logging/log_namer.cc
+++ b/aos/events/logging/log_namer.cc
@@ -14,6 +14,8 @@
#include "flatbuffers/flatbuffers.h"
#include "glog/logging.h"
+DECLARE_int32(flush_size);
+
namespace aos {
namespace logger {
@@ -568,7 +570,12 @@
EventLoop *event_loop, const Node *node)
: LogNamer(configuration, event_loop, node),
base_name_(base_name),
- old_base_name_() {}
+ old_base_name_(),
+ 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,
+ FLAGS_flush_size);
+ }) {}
MultiNodeLogNamer::~MultiNodeLogNamer() {
if (!ran_out_of_space_) {
diff --git a/aos/events/logging/log_namer.h b/aos/events/logging/log_namer.h
index 4e8c990..07edeac 100644
--- a/aos/events/logging/log_namer.h
+++ b/aos/events/logging/log_namer.h
@@ -491,10 +491,7 @@
std::vector<std::string> all_filenames_;
std::string temp_suffix_;
- std::function<std::unique_ptr<DataEncoder>(size_t)> encoder_factory_ =
- [](size_t max_message_size) {
- return std::make_unique<DummyEncoder>(max_message_size);
- };
+ std::function<std::unique_ptr<DataEncoder>(size_t)> encoder_factory_;
std::string extension_;
// Storage for statistics from previously-rotated DetachedBufferWriters.
diff --git a/aos/events/logging/log_writer.cc b/aos/events/logging/log_writer.cc
index 5671a3f..84f7503 100644
--- a/aos/events/logging/log_writer.cc
+++ b/aos/events/logging/log_writer.cc
@@ -766,64 +766,6 @@
}
}
-// Class to copy a context into the provided buffer.
-class ContextDataCopier : public DataEncoder::Copier {
- public:
- ContextDataCopier(const Context &context, int channel_index, LogType log_type,
- EventLoop *event_loop)
- : DataEncoder::Copier(PackMessageSize(log_type, context.size)),
- context_(context),
- channel_index_(channel_index),
- log_type_(log_type),
- event_loop_(event_loop) {}
-
- monotonic_clock::time_point end_time() const { return end_time_; }
-
- size_t Copy(uint8_t *data) final {
- size_t result =
- PackMessageInline(data, context_, channel_index_, log_type_);
- end_time_ = event_loop_->monotonic_now();
- return result;
- }
-
- private:
- const Context &context_;
- const int channel_index_;
- const LogType log_type_;
- EventLoop *event_loop_;
- monotonic_clock::time_point end_time_;
-};
-
-// Class to copy a RemoteMessage into the provided buffer.
-class RemoteMessageCopier : public DataEncoder::Copier {
- public:
- RemoteMessageCopier(const message_bridge::RemoteMessage *message,
- int channel_index,
- aos::monotonic_clock::time_point monotonic_timestamp_time,
- EventLoop *event_loop)
- : DataEncoder::Copier(PackRemoteMessageSize()),
- message_(message),
- channel_index_(channel_index),
- monotonic_timestamp_time_(monotonic_timestamp_time),
- event_loop_(event_loop) {}
-
- monotonic_clock::time_point end_time() const { return end_time_; }
-
- size_t Copy(uint8_t *data) final {
- size_t result = PackRemoteMessageInline(data, message_, channel_index_,
- monotonic_timestamp_time_);
- end_time_ = event_loop_->monotonic_now();
- return result;
- }
-
- private:
- const message_bridge::RemoteMessage *message_;
- int channel_index_;
- aos::monotonic_clock::time_point monotonic_timestamp_time_;
- EventLoop *event_loop_;
- monotonic_clock::time_point end_time_;
-};
-
void Logger::WriteData(NewDataWriter *writer, const FetcherStruct &f) {
if (writer != nullptr) {
const UUID source_node_boot_uuid =
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 90220c8..02a9fdd 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -64,6 +64,10 @@
"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 {
@@ -81,7 +85,10 @@
DetachedBufferWriter::DetachedBufferWriter(std::string_view filename,
std::unique_ptr<DataEncoder> encoder)
- : filename_(filename), encoder_(std::move(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 {
@@ -92,10 +99,38 @@
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() {
Close();
if (ran_out_of_space_) {
@@ -126,10 +161,14 @@
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;
}
-void DetachedBufferWriter::CopyMessage(DataEncoder::Copier *coppier,
+void DetachedBufferWriter::CopyMessage(DataEncoder::Copier *copier,
aos::monotonic_clock::time_point now) {
if (ran_out_of_space_) {
// We don't want any later data to be written after space becomes
@@ -138,12 +177,23 @@
return;
}
- if (!encoder_->HasSpace(coppier->size())) {
- Flush();
- CHECK(encoder_->HasSpace(coppier->size()));
- }
+ const size_t message_size = copier->size();
+ size_t overall_bytes_written = 0;
- encoder_->Encode(coppier);
+ // 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);
+ CHECK(bytes_written != 0);
+
+ overall_bytes_written += bytes_written;
+ if (overall_bytes_written < message_size) {
+ VLOG(1) << "Flushing because of a partial write, tried to write "
+ << message_size << " wrote " << overall_bytes_written;
+ Flush(now);
+ }
+ } while (overall_bytes_written < message_size);
+
FlushAtThreshold(now);
}
@@ -153,7 +203,7 @@
}
encoder_->Finish();
while (encoder_->queue_size() > 0) {
- Flush();
+ Flush(monotonic_clock::max_time);
}
if (close(fd_) == -1) {
if (errno == ENOSPC) {
@@ -166,7 +216,8 @@
VLOG(1) << "Closed " << filename();
}
-void DetachedBufferWriter::Flush() {
+void DetachedBufferWriter::Flush(aos::monotonic_clock::time_point now) {
+ last_flush_time_ = now;
if (ran_out_of_space_) {
// We don't want any later data to be written after space becomes available,
// so refuse to write anything more once we've dropped data because we ran
@@ -186,23 +237,110 @@
}
iovec_.clear();
- const size_t iovec_size = std::min<size_t>(queue.size(), IOV_MAX);
+ 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_[i].iov_base = const_cast<uint8_t *>(queue[i].data());
- iovec_[i].iov_len = queue[i].size();
- counted_size += iovec_[i].iov_len;
+ 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());
+ 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);
- encoder_->Clear(iovec_size);
-
UpdateStatsForWrite(end - start, written, iovec_size);
+
+ return written;
}
void DetachedBufferWriter::HandleWriteReturn(ssize_t write_return,
@@ -211,7 +349,7 @@
ran_out_of_space_ = true;
return;
}
- PCHECK(write_return >= 0) << ": write failed";
+ 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
@@ -260,13 +398,14 @@
// Flush if we are at the max number of iovs per writev, because there's no
// point queueing up any more data in memory. Also flush once we have enough
// data queued up or if it has been long enough.
- while (encoder_->queued_bytes() > static_cast<size_t>(FLAGS_flush_size) ||
+ 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))) {
- last_flush_time_ = now;
- Flush();
+ VLOG(1) << "Chose to flush at " << now << ", last " << last_flush_time_;
+ Flush(now);
}
}
@@ -323,73 +462,144 @@
size_t PackRemoteMessageInline(
uint8_t *buffer, const message_bridge::RemoteMessage *msg,
int channel_index,
- const aos::monotonic_clock::time_point monotonic_timestamp_time) {
+ const aos::monotonic_clock::time_point monotonic_timestamp_time,
+ size_t start_byte, size_t end_byte) {
const flatbuffers::uoffset_t message_size = PackRemoteMessageSize();
+ DCHECK_EQ((start_byte % 8u), 0u);
+ DCHECK_EQ((end_byte % 8u), 0u);
+ DCHECK_LE(start_byte, end_byte);
+ DCHECK_LE(end_byte, message_size);
- // clang-format off
- // header:
- // +0x00 | 5C 00 00 00 | UOffset32 | 0x0000005C (92) Loc: +0x5C | size prefix
- buffer = Push<flatbuffers::uoffset_t>(
- buffer, message_size - sizeof(flatbuffers::uoffset_t));
- // +0x04 | 20 00 00 00 | UOffset32 | 0x00000020 (32) Loc: +0x24 | offset to root table `aos.logger.MessageHeader`
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x20);
- //
- // padding:
- // +0x08 | 00 00 00 00 00 00 | uint8_t[6] | ...... | padding
- buffer = Pad(buffer, 6);
- //
- // vtable (aos.logger.MessageHeader):
- // +0x0E | 16 00 | uint16_t | 0x0016 (22) | size of this vtable
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x16);
- // +0x10 | 3C 00 | uint16_t | 0x003C (60) | size of referring table
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x3c);
- // +0x12 | 38 00 | VOffset16 | 0x0038 (56) | offset to field `channel_index` (id: 0)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x38);
- // +0x14 | 2C 00 | VOffset16 | 0x002C (44) | offset to field `monotonic_sent_time` (id: 1)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x2c);
- // +0x16 | 24 00 | VOffset16 | 0x0024 (36) | offset to field `realtime_sent_time` (id: 2)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x24);
- // +0x18 | 34 00 | VOffset16 | 0x0034 (52) | offset to field `queue_index` (id: 3)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x34);
- // +0x1A | 00 00 | VOffset16 | 0x0000 (0) | offset to field `data` (id: 4) <null> (Vector)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x00);
- // +0x1C | 1C 00 | VOffset16 | 0x001C (28) | offset to field `monotonic_remote_time` (id: 5)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x1c);
- // +0x1E | 14 00 | VOffset16 | 0x0014 (20) | offset to field `realtime_remote_time` (id: 6)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x14);
- // +0x20 | 10 00 | VOffset16 | 0x0010 (16) | offset to field `remote_queue_index` (id: 7)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x10);
- // +0x22 | 04 00 | VOffset16 | 0x0004 (4) | offset to field `monotonic_timestamp_time` (id: 8)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x04);
- //
- // root_table (aos.logger.MessageHeader):
- // +0x24 | 16 00 00 00 | SOffset32 | 0x00000016 (22) Loc: +0x0E | offset to vtable
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x16);
- // +0x28 | F6 0B D8 11 A4 A8 B1 71 | int64_t | 0x71B1A8A411D80BF6 (8192514619791117302) | table field `monotonic_timestamp_time` (Long)
- buffer = Push<int64_t>(buffer,
- monotonic_timestamp_time.time_since_epoch().count());
- // +0x30 | 00 00 00 00 | uint8_t[4] | .... | padding
- // TODO(austin): Can we re-arrange the order to ditch the padding?
- // (Answer is yes, but what is the impact elsewhere? It will change the
- // binary format)
- buffer = Pad(buffer, 4);
- // +0x34 | 75 00 00 00 | uint32_t | 0x00000075 (117) | table field `remote_queue_index` (UInt)
- buffer = Push<uint32_t>(buffer, msg->remote_queue_index());
- // +0x38 | AA B0 43 0A 35 BE FA D2 | int64_t | 0xD2FABE350A43B0AA (-3244071446552268630) | table field `realtime_remote_time` (Long)
- buffer = Push<int64_t>(buffer, msg->realtime_remote_time());
- // +0x40 | D5 40 30 F3 C1 A7 26 1D | int64_t | 0x1D26A7C1F33040D5 (2100550727665467605) | table field `monotonic_remote_time` (Long)
- buffer = Push<int64_t>(buffer, msg->monotonic_remote_time());
- // +0x48 | 5B 25 32 A1 4A E8 46 CA | int64_t | 0xCA46E84AA132255B (-3871151422448720549) | table field `realtime_sent_time` (Long)
- buffer = Push<int64_t>(buffer, msg->realtime_sent_time());
- // +0x50 | 49 7D 45 1F 8C 36 6B A3 | int64_t | 0xA36B368C1F457D49 (-6671178447571288759) | table field `monotonic_sent_time` (Long)
- buffer = Push<int64_t>(buffer, msg->monotonic_sent_time());
- // +0x58 | 33 00 00 00 | uint32_t | 0x00000033 (51) | table field `queue_index` (UInt)
- buffer = Push<uint32_t>(buffer, msg->queue_index());
- // +0x5C | 76 00 00 00 | uint32_t | 0x00000076 (118) | table field `channel_index` (UInt)
- buffer = Push<uint32_t>(buffer, channel_index);
- // clang-format on
+ switch (start_byte) {
+ case 0x00u:
+ if ((end_byte) == 0x00u) {
+ break;
+ }
+ // clang-format off
+ // header:
+ // +0x00 | 5C 00 00 00 | UOffset32 | 0x0000005C (92) Loc: +0x5C | size prefix
+ buffer = Push<flatbuffers::uoffset_t>(
+ buffer, message_size - sizeof(flatbuffers::uoffset_t));
+ // +0x04 | 20 00 00 00 | UOffset32 | 0x00000020 (32) Loc: +0x24 | offset to root table `aos.logger.MessageHeader`
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x20);
+ [[fallthrough]];
+ case 0x08u:
+ if ((end_byte) == 0x08u) {
+ break;
+ }
+ //
+ // padding:
+ // +0x08 | 00 00 00 00 00 00 | uint8_t[6] | ...... | padding
+ buffer = Pad(buffer, 6);
+ //
+ // vtable (aos.logger.MessageHeader):
+ // +0x0E | 16 00 | uint16_t | 0x0016 (22) | size of this vtable
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x16);
+ [[fallthrough]];
+ case 0x10u:
+ if ((end_byte) == 0x10u) {
+ break;
+ }
+ // +0x10 | 3C 00 | uint16_t | 0x003C (60) | size of referring table
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x3c);
+ // +0x12 | 38 00 | VOffset16 | 0x0038 (56) | offset to field `channel_index` (id: 0)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x38);
+ // +0x14 | 2C 00 | VOffset16 | 0x002C (44) | offset to field `monotonic_sent_time` (id: 1)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x2c);
+ // +0x16 | 24 00 | VOffset16 | 0x0024 (36) | offset to field `realtime_sent_time` (id: 2)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x24);
+ [[fallthrough]];
+ case 0x18u:
+ if ((end_byte) == 0x18u) {
+ break;
+ }
+ // +0x18 | 34 00 | VOffset16 | 0x0034 (52) | offset to field `queue_index` (id: 3)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x34);
+ // +0x1A | 00 00 | VOffset16 | 0x0000 (0) | offset to field `data` (id: 4) <null> (Vector)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x00);
+ // +0x1C | 1C 00 | VOffset16 | 0x001C (28) | offset to field `monotonic_remote_time` (id: 5)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x1c);
+ // +0x1E | 14 00 | VOffset16 | 0x0014 (20) | offset to field `realtime_remote_time` (id: 6)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x14);
+ [[fallthrough]];
+ case 0x20u:
+ if ((end_byte) == 0x20u) {
+ break;
+ }
+ // +0x20 | 10 00 | VOffset16 | 0x0010 (16) | offset to field `remote_queue_index` (id: 7)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x10);
+ // +0x22 | 04 00 | VOffset16 | 0x0004 (4) | offset to field `monotonic_timestamp_time` (id: 8)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x04);
+ //
+ // root_table (aos.logger.MessageHeader):
+ // +0x24 | 16 00 00 00 | SOffset32 | 0x00000016 (22) Loc: +0x0E | offset to vtable
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x16);
+ [[fallthrough]];
+ case 0x28u:
+ if ((end_byte) == 0x28u) {
+ break;
+ }
+ // +0x28 | F6 0B D8 11 A4 A8 B1 71 | int64_t | 0x71B1A8A411D80BF6 (8192514619791117302) | table field `monotonic_timestamp_time` (Long)
+ buffer = Push<int64_t>(buffer,
+ monotonic_timestamp_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x30u:
+ if ((end_byte) == 0x30u) {
+ break;
+ }
+ // +0x30 | 00 00 00 00 | uint8_t[4] | .... | padding
+ // TODO(austin): Can we re-arrange the order to ditch the padding?
+ // (Answer is yes, but what is the impact elsewhere? It will change the
+ // binary format)
+ buffer = Pad(buffer, 4);
+ // +0x34 | 75 00 00 00 | uint32_t | 0x00000075 (117) | table field `remote_queue_index` (UInt)
+ buffer = Push<uint32_t>(buffer, msg->remote_queue_index());
+ [[fallthrough]];
+ case 0x38u:
+ if ((end_byte) == 0x38u) {
+ break;
+ }
+ // +0x38 | AA B0 43 0A 35 BE FA D2 | int64_t | 0xD2FABE350A43B0AA (-3244071446552268630) | table field `realtime_remote_time` (Long)
+ buffer = Push<int64_t>(buffer, msg->realtime_remote_time());
+ [[fallthrough]];
+ case 0x40u:
+ if ((end_byte) == 0x40u) {
+ break;
+ }
+ // +0x40 | D5 40 30 F3 C1 A7 26 1D | int64_t | 0x1D26A7C1F33040D5 (2100550727665467605) | table field `monotonic_remote_time` (Long)
+ buffer = Push<int64_t>(buffer, msg->monotonic_remote_time());
+ [[fallthrough]];
+ case 0x48u:
+ if ((end_byte) == 0x48u) {
+ break;
+ }
+ // +0x48 | 5B 25 32 A1 4A E8 46 CA | int64_t | 0xCA46E84AA132255B (-3871151422448720549) | table field `realtime_sent_time` (Long)
+ buffer = Push<int64_t>(buffer, msg->realtime_sent_time());
+ [[fallthrough]];
+ case 0x50u:
+ if ((end_byte) == 0x50u) {
+ break;
+ }
+ // +0x50 | 49 7D 45 1F 8C 36 6B A3 | int64_t | 0xA36B368C1F457D49 (-6671178447571288759) | table field `monotonic_sent_time` (Long)
+ buffer = Push<int64_t>(buffer, msg->monotonic_sent_time());
+ [[fallthrough]];
+ case 0x58u:
+ if ((end_byte) == 0x58u) {
+ break;
+ }
+ // +0x58 | 33 00 00 00 | uint32_t | 0x00000033 (51) | table field `queue_index` (UInt)
+ buffer = Push<uint32_t>(buffer, msg->queue_index());
+ // +0x5C | 76 00 00 00 | uint32_t | 0x00000076 (118) | table field `channel_index` (UInt)
+ buffer = Push<uint32_t>(buffer, channel_index);
+ // clang-format on
+ [[fallthrough]];
+ case 0x60u:
+ if ((end_byte) == 0x60u) {
+ break;
+ }
+ }
- return message_size;
+ return end_byte - start_byte;
}
flatbuffers::Offset<MessageHeader> PackMessage(
@@ -580,271 +790,565 @@
}
size_t PackMessageInline(uint8_t *buffer, const Context &context,
- int channel_index, LogType log_type) {
+ int channel_index, LogType log_type, size_t start_byte,
+ size_t end_byte) {
// TODO(austin): Figure out how to copy directly from shared memory instead of
// first into the fetcher's memory and then into here. That would save a lot
// of memory.
const flatbuffers::uoffset_t message_size =
PackMessageSize(log_type, context.size);
-
- buffer = Push<flatbuffers::uoffset_t>(
- buffer, message_size - sizeof(flatbuffers::uoffset_t));
+ DCHECK_EQ((message_size % 8), 0u) << ": Non 8 byte length...";
+ DCHECK_EQ((start_byte % 8u), 0u);
+ DCHECK_EQ((end_byte % 8u), 0u);
+ DCHECK_LE(start_byte, end_byte);
+ DCHECK_LE(end_byte, message_size);
// Pack all the data in. This is brittle but easy to change. Use the
// InlinePackMessage.Equivilent unit test to verify everything matches.
switch (log_type) {
case LogType::kLogMessage:
- // clang-format off
- // header:
- // +0x00 | 4C 00 00 00 | UOffset32 | 0x0000004C (76) Loc: +0x4C | size prefix
- // +0x04 | 18 00 00 00 | UOffset32 | 0x00000018 (24) Loc: +0x1C | offset to root table `aos.logger.MessageHeader`
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x18);
- //
- // padding:
- // +0x08 | 00 00 00 00 00 00 | uint8_t[6] | ...... | padding
- buffer = Pad(buffer, 6);
- //
- // vtable (aos.logger.MessageHeader):
- // +0x0E | 0E 00 | uint16_t | 0x000E (14) | size of this vtable
- buffer = Push<flatbuffers::voffset_t>(buffer, 0xe);
- // +0x10 | 20 00 | uint16_t | 0x0020 (32) | size of referring table
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x20);
- // +0x12 | 1C 00 | VOffset16 | 0x001C (28) | offset to field `channel_index` (id: 0)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x1c);
- // +0x14 | 0C 00 | VOffset16 | 0x000C (12) | offset to field `monotonic_sent_time` (id: 1)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x0c);
- // +0x16 | 04 00 | VOffset16 | 0x0004 (4) | offset to field `realtime_sent_time` (id: 2)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x04);
- // +0x18 | 18 00 | VOffset16 | 0x0018 (24) | offset to field `queue_index` (id: 3)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x18);
- // +0x1A | 14 00 | VOffset16 | 0x0014 (20) | offset to field `data` (id: 4)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x14);
- //
- // root_table (aos.logger.MessageHeader):
- // +0x1C | 0E 00 00 00 | SOffset32 | 0x0000000E (14) Loc: +0x0E | offset to vtable
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x0e);
- // +0x20 | B2 E4 EF 89 19 7D 7F 6F | int64_t | 0x6F7F7D1989EFE4B2 (8034277808894108850) | table field `realtime_sent_time` (Long)
- buffer = Push<int64_t>(buffer, context.realtime_event_time.time_since_epoch().count());
- // +0x28 | 86 8D 92 65 FC 79 74 2B | int64_t | 0x2B7479FC65928D86 (3131261765872160134) | table field `monotonic_sent_time` (Long)
- buffer = Push<int64_t>(buffer, context.monotonic_event_time.time_since_epoch().count());
- // +0x30 | 0C 00 00 00 | UOffset32 | 0x0000000C (12) Loc: +0x3C | offset to field `data` (vector)
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x0c);
- // +0x34 | 86 00 00 00 | uint32_t | 0x00000086 (134) | table field `queue_index` (UInt)
- buffer = Push<uint32_t>(buffer, context.queue_index);
- // +0x38 | 71 00 00 00 | uint32_t | 0x00000071 (113) | table field `channel_index` (UInt)
- buffer = Push<uint32_t>(buffer, channel_index);
- //
- // vector (aos.logger.MessageHeader.data):
- // +0x3C | 0E 00 00 00 | uint32_t | 0x0000000E (14) | length of vector (# items)
- buffer = Push<flatbuffers::uoffset_t>(buffer, context.size);
- // +0x40 | FF | uint8_t | 0xFF (255) | value[0]
- // +0x41 | B8 | uint8_t | 0xB8 (184) | value[1]
- // +0x42 | EE | uint8_t | 0xEE (238) | value[2]
- // +0x43 | 00 | uint8_t | 0x00 (0) | value[3]
- // +0x44 | 20 | uint8_t | 0x20 (32) | value[4]
- // +0x45 | 4D | uint8_t | 0x4D (77) | value[5]
- // +0x46 | FF | uint8_t | 0xFF (255) | value[6]
- // +0x47 | 25 | uint8_t | 0x25 (37) | value[7]
- // +0x48 | 3C | uint8_t | 0x3C (60) | value[8]
- // +0x49 | 17 | uint8_t | 0x17 (23) | value[9]
- // +0x4A | 65 | uint8_t | 0x65 (101) | value[10]
- // +0x4B | 2F | uint8_t | 0x2F (47) | value[11]
- // +0x4C | 63 | uint8_t | 0x63 (99) | value[12]
- // +0x4D | 58 | uint8_t | 0x58 (88) | value[13]
- buffer = PushBytes(buffer, context.data, context.size);
- //
- // padding:
- // +0x4E | 00 00 | uint8_t[2] | .. | padding
- buffer = Pad(buffer, ((context.size + 7) & 0xfffffff8u) - context.size);
- // clang-format on
+ switch (start_byte) {
+ case 0x00u:
+ if ((end_byte) == 0x00u) {
+ break;
+ }
+ // clang-format off
+ // header:
+ // +0x00 | 4C 00 00 00 | UOffset32 | 0x0000004C (76) Loc: +0x4C | size prefix
+ buffer = Push<flatbuffers::uoffset_t>(
+ buffer, message_size - sizeof(flatbuffers::uoffset_t));
+
+ // +0x04 | 18 00 00 00 | UOffset32 | 0x00000018 (24) Loc: +0x1C | offset to root table `aos.logger.MessageHeader`
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x18);
+ [[fallthrough]];
+ case 0x08u:
+ if ((end_byte) == 0x08u) {
+ break;
+ }
+ //
+ // padding:
+ // +0x08 | 00 00 00 00 00 00 | uint8_t[6] | ...... | padding
+ buffer = Pad(buffer, 6);
+ //
+ // vtable (aos.logger.MessageHeader):
+ // +0x0E | 0E 00 | uint16_t | 0x000E (14) | size of this vtable
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0xe);
+ [[fallthrough]];
+ case 0x10u:
+ if ((end_byte) == 0x10u) {
+ break;
+ }
+ // +0x10 | 20 00 | uint16_t | 0x0020 (32) | size of referring table
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x20);
+ // +0x12 | 1C 00 | VOffset16 | 0x001C (28) | offset to field `channel_index` (id: 0)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x1c);
+ // +0x14 | 0C 00 | VOffset16 | 0x000C (12) | offset to field `monotonic_sent_time` (id: 1)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x0c);
+ // +0x16 | 04 00 | VOffset16 | 0x0004 (4) | offset to field `realtime_sent_time` (id: 2)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x04);
+ [[fallthrough]];
+ case 0x18u:
+ if ((end_byte) == 0x18u) {
+ break;
+ }
+ // +0x18 | 18 00 | VOffset16 | 0x0018 (24) | offset to field `queue_index` (id: 3)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x18);
+ // +0x1A | 14 00 | VOffset16 | 0x0014 (20) | offset to field `data` (id: 4)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x14);
+ //
+ // root_table (aos.logger.MessageHeader):
+ // +0x1C | 0E 00 00 00 | SOffset32 | 0x0000000E (14) Loc: +0x0E | offset to vtable
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x0e);
+ [[fallthrough]];
+ case 0x20u:
+ if ((end_byte) == 0x20u) {
+ break;
+ }
+ // +0x20 | B2 E4 EF 89 19 7D 7F 6F | int64_t | 0x6F7F7D1989EFE4B2 (8034277808894108850) | table field `realtime_sent_time` (Long)
+ buffer = Push<int64_t>(buffer, context.realtime_event_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x28u:
+ if ((end_byte) == 0x28u) {
+ break;
+ }
+ // +0x28 | 86 8D 92 65 FC 79 74 2B | int64_t | 0x2B7479FC65928D86 (3131261765872160134) | table field `monotonic_sent_time` (Long)
+ buffer = Push<int64_t>(buffer, context.monotonic_event_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x30u:
+ if ((end_byte) == 0x30u) {
+ break;
+ }
+ // +0x30 | 0C 00 00 00 | UOffset32 | 0x0000000C (12) Loc: +0x3C | offset to field `data` (vector)
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x0c);
+ // +0x34 | 86 00 00 00 | uint32_t | 0x00000086 (134) | table field `queue_index` (UInt)
+ buffer = Push<uint32_t>(buffer, context.queue_index);
+ [[fallthrough]];
+ case 0x38u:
+ if ((end_byte) == 0x38u) {
+ break;
+ }
+ // +0x38 | 71 00 00 00 | uint32_t | 0x00000071 (113) | table field `channel_index` (UInt)
+ buffer = Push<uint32_t>(buffer, channel_index);
+ //
+ // vector (aos.logger.MessageHeader.data):
+ // +0x3C | 0E 00 00 00 | uint32_t | 0x0000000E (14) | length of vector (# items)
+ buffer = Push<flatbuffers::uoffset_t>(buffer, context.size);
+ [[fallthrough]];
+ case 0x40u:
+ if ((end_byte) == 0x40u) {
+ break;
+ }
+ [[fallthrough]];
+ default:
+ // +0x40 | FF | uint8_t | 0xFF (255) | value[0]
+ // +0x41 | B8 | uint8_t | 0xB8 (184) | value[1]
+ // +0x42 | EE | uint8_t | 0xEE (238) | value[2]
+ // +0x43 | 00 | uint8_t | 0x00 (0) | value[3]
+ // +0x44 | 20 | uint8_t | 0x20 (32) | value[4]
+ // +0x45 | 4D | uint8_t | 0x4D (77) | value[5]
+ // +0x46 | FF | uint8_t | 0xFF (255) | value[6]
+ // +0x47 | 25 | uint8_t | 0x25 (37) | value[7]
+ // +0x48 | 3C | uint8_t | 0x3C (60) | value[8]
+ // +0x49 | 17 | uint8_t | 0x17 (23) | value[9]
+ // +0x4A | 65 | uint8_t | 0x65 (101) | value[10]
+ // +0x4B | 2F | uint8_t | 0x2F (47) | value[11]
+ // +0x4C | 63 | uint8_t | 0x63 (99) | value[12]
+ // +0x4D | 58 | uint8_t | 0x58 (88) | value[13]
+ //
+ // padding:
+ // +0x4E | 00 00 | uint8_t[2] | .. | padding
+ // clang-format on
+ if (start_byte <= 0x40 && end_byte == message_size) {
+ // The easy one, slap it all down.
+ buffer = PushBytes(buffer, context.data, context.size);
+ buffer =
+ Pad(buffer, ((context.size + 7) & 0xfffffff8u) - context.size);
+ } else {
+ const size_t data_start_byte =
+ start_byte < 0x40 ? 0x0u : (start_byte - 0x40);
+ const size_t data_end_byte = end_byte - 0x40;
+ const size_t padded_size = ((context.size + 7) & 0xfffffff8u);
+ if (data_start_byte < padded_size) {
+ buffer = PushBytes(
+ buffer,
+ reinterpret_cast<const uint8_t *>(context.data) +
+ data_start_byte,
+ std::min(context.size, data_end_byte) - data_start_byte);
+ if (data_end_byte == padded_size) {
+ // We can only pad the last 7 bytes, so this only gets written
+ // if we write the last byte.
+ buffer = Pad(buffer,
+ ((context.size + 7) & 0xfffffff8u) - context.size);
+ }
+ }
+ }
+ break;
+ }
break;
case LogType::kLogDeliveryTimeOnly:
- // clang-format off
- // header:
- // +0x00 | 4C 00 00 00 | UOffset32 | 0x0000004C (76) Loc: +0x4C | size prefix
- // +0x04 | 1C 00 00 00 | UOffset32 | 0x0000001C (28) Loc: +0x20 | offset to root table `aos.logger.MessageHeader`
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x1c);
- //
- // padding:
- // +0x08 | 00 00 00 00 | uint8_t[4] | .... | padding
- buffer = Pad(buffer, 4);
- //
- // vtable (aos.logger.MessageHeader):
- // +0x0C | 14 00 | uint16_t | 0x0014 (20) | size of this vtable
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x14);
- // +0x0E | 30 00 | uint16_t | 0x0030 (48) | size of referring table
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x30);
- // +0x10 | 2C 00 | VOffset16 | 0x002C (44) | offset to field `channel_index` (id: 0)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x2c);
- // +0x12 | 20 00 | VOffset16 | 0x0020 (32) | offset to field `monotonic_sent_time` (id: 1)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x20);
- // +0x14 | 18 00 | VOffset16 | 0x0018 (24) | offset to field `realtime_sent_time` (id: 2)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x18);
- // +0x16 | 28 00 | VOffset16 | 0x0028 (40) | offset to field `queue_index` (id: 3)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x28);
- // +0x18 | 00 00 | VOffset16 | 0x0000 (0) | offset to field `data` (id: 4) <null> (Vector)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x00);
- // +0x1A | 10 00 | VOffset16 | 0x0010 (16) | offset to field `monotonic_remote_time` (id: 5)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x10);
- // +0x1C | 08 00 | VOffset16 | 0x0008 (8) | offset to field `realtime_remote_time` (id: 6)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x08);
- // +0x1E | 04 00 | VOffset16 | 0x0004 (4) | offset to field `remote_queue_index` (id: 7)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x04);
- //
- // root_table (aos.logger.MessageHeader):
- // +0x20 | 14 00 00 00 | SOffset32 | 0x00000014 (20) Loc: +0x0C | offset to vtable
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x14);
- // +0x24 | 69 00 00 00 | uint32_t | 0x00000069 (105) | table field `remote_queue_index` (UInt)
- buffer = Push<uint32_t>(buffer, context.remote_queue_index);
- // +0x28 | C6 85 F1 AB 83 B5 CD EB | int64_t | 0xEBCDB583ABF185C6 (-1455307527440726586) | table field `realtime_remote_time` (Long)
- buffer = Push<int64_t>(buffer, context.realtime_remote_time.time_since_epoch().count());
- // +0x30 | 47 24 D3 97 1E 42 2D 99 | int64_t | 0x992D421E97D32447 (-7409193112790948793) | table field `monotonic_remote_time` (Long)
- buffer = Push<int64_t>(buffer, context.monotonic_remote_time.time_since_epoch().count());
- // +0x38 | C8 B9 A7 AB 79 F2 CD 60 | int64_t | 0x60CDF279ABA7B9C8 (6975498002251626952) | table field `realtime_sent_time` (Long)
- buffer = Push<int64_t>(buffer, context.realtime_event_time.time_since_epoch().count());
- // +0x40 | EA 8F 2A 0F AF 01 7A AB | int64_t | 0xAB7A01AF0F2A8FEA (-6090553694679822358) | table field `monotonic_sent_time` (Long)
- buffer = Push<int64_t>(buffer, context.monotonic_event_time.time_since_epoch().count());
- // +0x48 | F5 00 00 00 | uint32_t | 0x000000F5 (245) | table field `queue_index` (UInt)
- buffer = Push<uint32_t>(buffer, context.queue_index);
- // +0x4C | 88 00 00 00 | uint32_t | 0x00000088 (136) | table field `channel_index` (UInt)
- buffer = Push<uint32_t>(buffer, channel_index);
+ switch (start_byte) {
+ case 0x00u:
+ if ((end_byte) == 0x00u) {
+ break;
+ }
+ // clang-format off
+ // header:
+ // +0x00 | 4C 00 00 00 | UOffset32 | 0x0000004C (76) Loc: +0x4C | size prefix
+ buffer = Push<flatbuffers::uoffset_t>(
+ buffer, message_size - sizeof(flatbuffers::uoffset_t));
+ // +0x04 | 1C 00 00 00 | UOffset32 | 0x0000001C (28) Loc: +0x20 | offset to root table `aos.logger.MessageHeader`
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x1c);
- // clang-format on
+ [[fallthrough]];
+ case 0x08u:
+ if ((end_byte) == 0x08u) {
+ break;
+ }
+ //
+ // padding:
+ // +0x08 | 00 00 00 00 | uint8_t[4] | .... | padding
+ buffer = Pad(buffer, 4);
+ //
+ // vtable (aos.logger.MessageHeader):
+ // +0x0C | 14 00 | uint16_t | 0x0014 (20) | size of this vtable
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x14);
+ // +0x0E | 30 00 | uint16_t | 0x0030 (48) | size of referring table
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x30);
+ [[fallthrough]];
+ case 0x10u:
+ if ((end_byte) == 0x10u) {
+ break;
+ }
+ // +0x10 | 2C 00 | VOffset16 | 0x002C (44) | offset to field `channel_index` (id: 0)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x2c);
+ // +0x12 | 20 00 | VOffset16 | 0x0020 (32) | offset to field `monotonic_sent_time` (id: 1)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x20);
+ // +0x14 | 18 00 | VOffset16 | 0x0018 (24) | offset to field `realtime_sent_time` (id: 2)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x18);
+ // +0x16 | 28 00 | VOffset16 | 0x0028 (40) | offset to field `queue_index` (id: 3)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x28);
+ [[fallthrough]];
+ case 0x18u:
+ if ((end_byte) == 0x18u) {
+ break;
+ }
+ // +0x18 | 00 00 | VOffset16 | 0x0000 (0) | offset to field `data` (id: 4) <null> (Vector)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x00);
+ // +0x1A | 10 00 | VOffset16 | 0x0010 (16) | offset to field `monotonic_remote_time` (id: 5)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x10);
+ // +0x1C | 08 00 | VOffset16 | 0x0008 (8) | offset to field `realtime_remote_time` (id: 6)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x08);
+ // +0x1E | 04 00 | VOffset16 | 0x0004 (4) | offset to field `remote_queue_index` (id: 7)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x04);
+ [[fallthrough]];
+ case 0x20u:
+ if ((end_byte) == 0x20u) {
+ break;
+ }
+ //
+ // root_table (aos.logger.MessageHeader):
+ // +0x20 | 14 00 00 00 | SOffset32 | 0x00000014 (20) Loc: +0x0C | offset to vtable
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x14);
+ // +0x24 | 69 00 00 00 | uint32_t | 0x00000069 (105) | table field `remote_queue_index` (UInt)
+ buffer = Push<uint32_t>(buffer, context.remote_queue_index);
+ [[fallthrough]];
+ case 0x28u:
+ if ((end_byte) == 0x28u) {
+ break;
+ }
+ // +0x28 | C6 85 F1 AB 83 B5 CD EB | int64_t | 0xEBCDB583ABF185C6 (-1455307527440726586) | table field `realtime_remote_time` (Long)
+ buffer = Push<int64_t>(buffer, context.realtime_remote_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x30u:
+ if ((end_byte) == 0x30u) {
+ break;
+ }
+ // +0x30 | 47 24 D3 97 1E 42 2D 99 | int64_t | 0x992D421E97D32447 (-7409193112790948793) | table field `monotonic_remote_time` (Long)
+ buffer = Push<int64_t>(buffer, context.monotonic_remote_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x38u:
+ if ((end_byte) == 0x38u) {
+ break;
+ }
+ // +0x38 | C8 B9 A7 AB 79 F2 CD 60 | int64_t | 0x60CDF279ABA7B9C8 (6975498002251626952) | table field `realtime_sent_time` (Long)
+ buffer = Push<int64_t>(buffer, context.realtime_event_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x40u:
+ if ((end_byte) == 0x40u) {
+ break;
+ }
+ // +0x40 | EA 8F 2A 0F AF 01 7A AB | int64_t | 0xAB7A01AF0F2A8FEA (-6090553694679822358) | table field `monotonic_sent_time` (Long)
+ buffer = Push<int64_t>(buffer, context.monotonic_event_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x48u:
+ if ((end_byte) == 0x48u) {
+ break;
+ }
+ // +0x48 | F5 00 00 00 | uint32_t | 0x000000F5 (245) | table field `queue_index` (UInt)
+ buffer = Push<uint32_t>(buffer, context.queue_index);
+ // +0x4C | 88 00 00 00 | uint32_t | 0x00000088 (136) | table field `channel_index` (UInt)
+ buffer = Push<uint32_t>(buffer, channel_index);
+
+ // clang-format on
+ }
break;
case LogType::kLogMessageAndDeliveryTime:
- // clang-format off
- // header:
- // +0x00 | 5C 00 00 00 | UOffset32 | 0x0000005C (92) Loc: +0x5C | size prefix
- // +0x04 | 1C 00 00 00 | UOffset32 | 0x0000001C (28) Loc: +0x20 | offset to root table `aos.logger.MessageHeader`
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x1c);
- //
- // padding:
- // +0x08 | 00 00 00 00 | uint8_t[4] | .... | padding
- buffer = Pad(buffer, 4);
- //
- // vtable (aos.logger.MessageHeader):
- // +0x0C | 14 00 | uint16_t | 0x0014 (20) | size of this vtable
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x14);
- // +0x0E | 34 00 | uint16_t | 0x0034 (52) | size of referring table
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x34);
- // +0x10 | 30 00 | VOffset16 | 0x0030 (48) | offset to field `channel_index` (id: 0)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x30);
- // +0x12 | 20 00 | VOffset16 | 0x0020 (32) | offset to field `monotonic_sent_time` (id: 1)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x20);
- // +0x14 | 18 00 | VOffset16 | 0x0018 (24) | offset to field `realtime_sent_time` (id: 2)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x18);
- // +0x16 | 2C 00 | VOffset16 | 0x002C (44) | offset to field `queue_index` (id: 3)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x2c);
- // +0x18 | 04 00 | VOffset16 | 0x0004 (4) | offset to field `data` (id: 4)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x04);
- // +0x1A | 10 00 | VOffset16 | 0x0010 (16) | offset to field `monotonic_remote_time` (id: 5)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x10);
- // +0x1C | 08 00 | VOffset16 | 0x0008 (8) | offset to field `realtime_remote_time` (id: 6)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x08);
- // +0x1E | 28 00 | VOffset16 | 0x0028 (40) | offset to field `remote_queue_index` (id: 7)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x28);
- //
- // root_table (aos.logger.MessageHeader):
- // +0x20 | 14 00 00 00 | SOffset32 | 0x00000014 (20) Loc: +0x0C | offset to vtable
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x14);
- // +0x24 | 30 00 00 00 | UOffset32 | 0x00000030 (48) Loc: +0x54 | offset to field `data` (vector)
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x30);
- // +0x28 | C4 C8 87 BF 40 6C 1F 29 | int64_t | 0x291F6C40BF87C8C4 (2963206105180129476) | table field `realtime_remote_time` (Long)
- buffer = Push<int64_t>(buffer, context.realtime_remote_time.time_since_epoch().count());
- // +0x30 | 0F 00 26 FD D2 6D C0 1F | int64_t | 0x1FC06DD2FD26000F (2287949363661897743) | table field `monotonic_remote_time` (Long)
- buffer = Push<int64_t>(buffer, context.monotonic_remote_time.time_since_epoch().count());
- // +0x38 | 29 75 09 C0 73 73 BF 88 | int64_t | 0x88BF7373C0097529 (-8593022623019338455) | table field `realtime_sent_time` (Long)
- buffer = Push<int64_t>(buffer, context.realtime_event_time.time_since_epoch().count());
- // +0x40 | 6D 8A AE 04 50 25 9C E9 | int64_t | 0xE99C255004AE8A6D (-1613373540899321235) | table field `monotonic_sent_time` (Long)
- buffer = Push<int64_t>(buffer, context.monotonic_event_time.time_since_epoch().count());
- // +0x48 | 47 00 00 00 | uint32_t | 0x00000047 (71) | table field `remote_queue_index` (UInt)
- buffer = Push<uint32_t>(buffer, context.remote_queue_index);
- // +0x4C | 4C 00 00 00 | uint32_t | 0x0000004C (76) | table field `queue_index` (UInt)
- buffer = Push<uint32_t>(buffer, context.queue_index);
- // +0x50 | 72 00 00 00 | uint32_t | 0x00000072 (114) | table field `channel_index` (UInt)
- buffer = Push<uint32_t>(buffer, channel_index);
- //
- // vector (aos.logger.MessageHeader.data):
- // +0x54 | 07 00 00 00 | uint32_t | 0x00000007 (7) | length of vector (# items)
- buffer = Push<flatbuffers::uoffset_t>(buffer, context.size);
- // +0x58 | B1 | uint8_t | 0xB1 (177) | value[0]
- // +0x59 | 4A | uint8_t | 0x4A (74) | value[1]
- // +0x5A | 50 | uint8_t | 0x50 (80) | value[2]
- // +0x5B | 24 | uint8_t | 0x24 (36) | value[3]
- // +0x5C | AF | uint8_t | 0xAF (175) | value[4]
- // +0x5D | C8 | uint8_t | 0xC8 (200) | value[5]
- // +0x5E | D5 | uint8_t | 0xD5 (213) | value[6]
- buffer = PushBytes(buffer, context.data, context.size);
- //
- // padding:
- // +0x5F | 00 | uint8_t[1] | . | padding
- buffer = Pad(buffer, ((context.size + 7) & 0xfffffff8u) - context.size);
- // clang-format on
+ switch (start_byte) {
+ case 0x00u:
+ if ((end_byte) == 0x00u) {
+ break;
+ }
+ // clang-format off
+ // header:
+ // +0x00 | 5C 00 00 00 | UOffset32 | 0x0000005C (92) Loc: +0x5C | size prefix
+ buffer = Push<flatbuffers::uoffset_t>(
+ buffer, message_size - sizeof(flatbuffers::uoffset_t));
+ // +0x04 | 1C 00 00 00 | UOffset32 | 0x0000001C (28) Loc: +0x20 | offset to root table `aos.logger.MessageHeader`
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x1c);
+ [[fallthrough]];
+ case 0x08u:
+ if ((end_byte) == 0x08u) {
+ break;
+ }
+ //
+ // padding:
+ // +0x08 | 00 00 00 00 | uint8_t[4] | .... | padding
+ buffer = Pad(buffer, 4);
+ //
+ // vtable (aos.logger.MessageHeader):
+ // +0x0C | 14 00 | uint16_t | 0x0014 (20) | size of this vtable
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x14);
+ // +0x0E | 34 00 | uint16_t | 0x0034 (52) | size of referring table
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x34);
+ [[fallthrough]];
+ case 0x10u:
+ if ((end_byte) == 0x10u) {
+ break;
+ }
+ // +0x10 | 30 00 | VOffset16 | 0x0030 (48) | offset to field `channel_index` (id: 0)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x30);
+ // +0x12 | 20 00 | VOffset16 | 0x0020 (32) | offset to field `monotonic_sent_time` (id: 1)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x20);
+ // +0x14 | 18 00 | VOffset16 | 0x0018 (24) | offset to field `realtime_sent_time` (id: 2)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x18);
+ // +0x16 | 2C 00 | VOffset16 | 0x002C (44) | offset to field `queue_index` (id: 3)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x2c);
+ [[fallthrough]];
+ case 0x18u:
+ if ((end_byte) == 0x18u) {
+ break;
+ }
+ // +0x18 | 04 00 | VOffset16 | 0x0004 (4) | offset to field `data` (id: 4)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x04);
+ // +0x1A | 10 00 | VOffset16 | 0x0010 (16) | offset to field `monotonic_remote_time` (id: 5)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x10);
+ // +0x1C | 08 00 | VOffset16 | 0x0008 (8) | offset to field `realtime_remote_time` (id: 6)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x08);
+ // +0x1E | 28 00 | VOffset16 | 0x0028 (40) | offset to field `remote_queue_index` (id: 7)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x28);
+ [[fallthrough]];
+ case 0x20u:
+ if ((end_byte) == 0x20u) {
+ break;
+ }
+ //
+ // root_table (aos.logger.MessageHeader):
+ // +0x20 | 14 00 00 00 | SOffset32 | 0x00000014 (20) Loc: +0x0C | offset to vtable
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x14);
+ // +0x24 | 30 00 00 00 | UOffset32 | 0x00000030 (48) Loc: +0x54 | offset to field `data` (vector)
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x30);
+ [[fallthrough]];
+ case 0x28u:
+ if ((end_byte) == 0x28u) {
+ break;
+ }
+ // +0x28 | C4 C8 87 BF 40 6C 1F 29 | int64_t | 0x291F6C40BF87C8C4 (2963206105180129476) | table field `realtime_remote_time` (Long)
+ buffer = Push<int64_t>(buffer, context.realtime_remote_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x30u:
+ if ((end_byte) == 0x30u) {
+ break;
+ }
+ // +0x30 | 0F 00 26 FD D2 6D C0 1F | int64_t | 0x1FC06DD2FD26000F (2287949363661897743) | table field `monotonic_remote_time` (Long)
+ buffer = Push<int64_t>(buffer, context.monotonic_remote_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x38u:
+ if ((end_byte) == 0x38u) {
+ break;
+ }
+ // +0x38 | 29 75 09 C0 73 73 BF 88 | int64_t | 0x88BF7373C0097529 (-8593022623019338455) | table field `realtime_sent_time` (Long)
+ buffer = Push<int64_t>(buffer, context.realtime_event_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x40u:
+ if ((end_byte) == 0x40u) {
+ break;
+ }
+ // +0x40 | 6D 8A AE 04 50 25 9C E9 | int64_t | 0xE99C255004AE8A6D (-1613373540899321235) | table field `monotonic_sent_time` (Long)
+ buffer = Push<int64_t>(buffer, context.monotonic_event_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x48u:
+ if ((end_byte) == 0x48u) {
+ break;
+ }
+ // +0x48 | 47 00 00 00 | uint32_t | 0x00000047 (71) | table field `remote_queue_index` (UInt)
+ buffer = Push<uint32_t>(buffer, context.remote_queue_index);
+ // +0x4C | 4C 00 00 00 | uint32_t | 0x0000004C (76) | table field `queue_index` (UInt)
+ buffer = Push<uint32_t>(buffer, context.queue_index);
+ [[fallthrough]];
+ case 0x50u:
+ if ((end_byte) == 0x50u) {
+ break;
+ }
+ // +0x50 | 72 00 00 00 | uint32_t | 0x00000072 (114) | table field `channel_index` (UInt)
+ buffer = Push<uint32_t>(buffer, channel_index);
+ //
+ // vector (aos.logger.MessageHeader.data):
+ // +0x54 | 07 00 00 00 | uint32_t | 0x00000007 (7) | length of vector (# items)
+ buffer = Push<flatbuffers::uoffset_t>(buffer, context.size);
+ [[fallthrough]];
+ case 0x58u:
+ if ((end_byte) == 0x58u) {
+ break;
+ }
+ [[fallthrough]];
+ default:
+ // +0x58 | B1 | uint8_t | 0xB1 (177) | value[0]
+ // +0x59 | 4A | uint8_t | 0x4A (74) | value[1]
+ // +0x5A | 50 | uint8_t | 0x50 (80) | value[2]
+ // +0x5B | 24 | uint8_t | 0x24 (36) | value[3]
+ // +0x5C | AF | uint8_t | 0xAF (175) | value[4]
+ // +0x5D | C8 | uint8_t | 0xC8 (200) | value[5]
+ // +0x5E | D5 | uint8_t | 0xD5 (213) | value[6]
+ //
+ // padding:
+ // +0x5F | 00 | uint8_t[1] | . | padding
+ // clang-format on
+
+ if (start_byte <= 0x58 && end_byte == message_size) {
+ // The easy one, slap it all down.
+ buffer = PushBytes(buffer, context.data, context.size);
+ buffer =
+ Pad(buffer, ((context.size + 7) & 0xfffffff8u) - context.size);
+ } else {
+ const size_t data_start_byte =
+ start_byte < 0x58 ? 0x0u : (start_byte - 0x58);
+ const size_t data_end_byte = end_byte - 0x58;
+ const size_t padded_size = ((context.size + 7) & 0xfffffff8u);
+ if (data_start_byte < padded_size) {
+ buffer = PushBytes(
+ buffer,
+ reinterpret_cast<const uint8_t *>(context.data) +
+ data_start_byte,
+ std::min(context.size, data_end_byte) - data_start_byte);
+ if (data_end_byte == padded_size) {
+ // We can only pad the last 7 bytes, so this only gets written
+ // if we write the last byte.
+ buffer = Pad(buffer,
+ ((context.size + 7) & 0xfffffff8u) - context.size);
+ }
+ }
+ }
+
+ break;
+ }
break;
case LogType::kLogRemoteMessage:
- // This is the message we need to recreate.
- //
- // clang-format off
- // header:
- // +0x00 | 5C 00 00 00 | UOffset32 | 0x0000005C (92) Loc: +0x5C | size prefix
- // +0x04 | 18 00 00 00 | UOffset32 | 0x00000018 (24) Loc: +0x1C | offset to root table `aos.logger.MessageHeader`
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x18);
- //
- // padding:
- // +0x08 | 00 00 00 00 00 00 | uint8_t[6] | ...... | padding
- buffer = Pad(buffer, 6);
- //
- // vtable (aos.logger.MessageHeader):
- // +0x0E | 0E 00 | uint16_t | 0x000E (14) | size of this vtable
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x0e);
- // +0x10 | 20 00 | uint16_t | 0x0020 (32) | size of referring table
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x20);
- // +0x12 | 1C 00 | VOffset16 | 0x001C (28) | offset to field `channel_index` (id: 0)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x1c);
- // +0x14 | 0C 00 | VOffset16 | 0x000C (12) | offset to field `monotonic_sent_time` (id: 1)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x0c);
- // +0x16 | 04 00 | VOffset16 | 0x0004 (4) | offset to field `realtime_sent_time` (id: 2)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x04);
- // +0x18 | 18 00 | VOffset16 | 0x0018 (24) | offset to field `queue_index` (id: 3)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x18);
- // +0x1A | 14 00 | VOffset16 | 0x0014 (20) | offset to field `data` (id: 4)
- buffer = Push<flatbuffers::voffset_t>(buffer, 0x14);
- //
- // root_table (aos.logger.MessageHeader):
- // +0x1C | 0E 00 00 00 | SOffset32 | 0x0000000E (14) Loc: +0x0E | offset to vtable
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x0E);
- // +0x20 | D8 96 32 1A A0 D3 23 BB | int64_t | 0xBB23D3A01A3296D8 (-4961889679844403496) | table field `realtime_sent_time` (Long)
- buffer = Push<int64_t>(buffer, context.realtime_remote_time.time_since_epoch().count());
- // +0x28 | 2E 5D 23 B3 BE 84 CF C2 | int64_t | 0xC2CF84BEB3235D2E (-4409159555588334290) | table field `monotonic_sent_time` (Long)
- buffer = Push<int64_t>(buffer, context.monotonic_remote_time.time_since_epoch().count());
- // +0x30 | 0C 00 00 00 | UOffset32 | 0x0000000C (12) Loc: +0x3C | offset to field `data` (vector)
- buffer = Push<flatbuffers::uoffset_t>(buffer, 0x0C);
- // +0x34 | 69 00 00 00 | uint32_t | 0x00000069 (105) | table field `queue_index` (UInt)
- buffer = Push<uint32_t>(buffer, context.remote_queue_index);
- // +0x38 | F3 00 00 00 | uint32_t | 0x000000F3 (243) | table field `channel_index` (UInt)
- buffer = Push<uint32_t>(buffer, channel_index);
- //
- // vector (aos.logger.MessageHeader.data):
- // +0x3C | 1A 00 00 00 | uint32_t | 0x0000001A (26) | length of vector (# items)
- buffer = Push<flatbuffers::uoffset_t>(buffer, context.size);
- // +0x40 | 38 | uint8_t | 0x38 (56) | value[0]
- // +0x41 | 1A | uint8_t | 0x1A (26) | value[1]
- // ...
- // +0x58 | 90 | uint8_t | 0x90 (144) | value[24]
- // +0x59 | 92 | uint8_t | 0x92 (146) | value[25]
- buffer = PushBytes(buffer, context.data, context.size);
- //
- // padding:
- // +0x5A | 00 00 00 00 00 00 | uint8_t[6] | ...... | padding
- buffer = Pad(buffer, ((context.size + 7) & 0xfffffff8u) - context.size);
- // clang-format on
+ switch (start_byte) {
+ case 0x00u:
+ if ((end_byte) == 0x00u) {
+ break;
+ }
+ // This is the message we need to recreate.
+ //
+ // clang-format off
+ // header:
+ // +0x00 | 5C 00 00 00 | UOffset32 | 0x0000005C (92) Loc: +0x5C | size prefix
+ buffer = Push<flatbuffers::uoffset_t>(
+ buffer, message_size - sizeof(flatbuffers::uoffset_t));
+ // +0x04 | 18 00 00 00 | UOffset32 | 0x00000018 (24) Loc: +0x1C | offset to root table `aos.logger.MessageHeader`
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x18);
+ [[fallthrough]];
+ case 0x08u:
+ if ((end_byte) == 0x08u) {
+ break;
+ }
+ //
+ // padding:
+ // +0x08 | 00 00 00 00 00 00 | uint8_t[6] | ...... | padding
+ buffer = Pad(buffer, 6);
+ //
+ // vtable (aos.logger.MessageHeader):
+ // +0x0E | 0E 00 | uint16_t | 0x000E (14) | size of this vtable
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x0e);
+ [[fallthrough]];
+ case 0x10u:
+ if ((end_byte) == 0x10u) {
+ break;
+ }
+ // +0x10 | 20 00 | uint16_t | 0x0020 (32) | size of referring table
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x20);
+ // +0x12 | 1C 00 | VOffset16 | 0x001C (28) | offset to field `channel_index` (id: 0)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x1c);
+ // +0x14 | 0C 00 | VOffset16 | 0x000C (12) | offset to field `monotonic_sent_time` (id: 1)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x0c);
+ // +0x16 | 04 00 | VOffset16 | 0x0004 (4) | offset to field `realtime_sent_time` (id: 2)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x04);
+ [[fallthrough]];
+ case 0x18u:
+ if ((end_byte) == 0x18u) {
+ break;
+ }
+ // +0x18 | 18 00 | VOffset16 | 0x0018 (24) | offset to field `queue_index` (id: 3)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x18);
+ // +0x1A | 14 00 | VOffset16 | 0x0014 (20) | offset to field `data` (id: 4)
+ buffer = Push<flatbuffers::voffset_t>(buffer, 0x14);
+ //
+ // root_table (aos.logger.MessageHeader):
+ // +0x1C | 0E 00 00 00 | SOffset32 | 0x0000000E (14) Loc: +0x0E | offset to vtable
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x0E);
+ [[fallthrough]];
+ case 0x20u:
+ if ((end_byte) == 0x20u) {
+ break;
+ }
+ // +0x20 | D8 96 32 1A A0 D3 23 BB | int64_t | 0xBB23D3A01A3296D8 (-4961889679844403496) | table field `realtime_sent_time` (Long)
+ buffer = Push<int64_t>(buffer, context.realtime_remote_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x28u:
+ if ((end_byte) == 0x28u) {
+ break;
+ }
+ // +0x28 | 2E 5D 23 B3 BE 84 CF C2 | int64_t | 0xC2CF84BEB3235D2E (-4409159555588334290) | table field `monotonic_sent_time` (Long)
+ buffer = Push<int64_t>(buffer, context.monotonic_remote_time.time_since_epoch().count());
+ [[fallthrough]];
+ case 0x30u:
+ if ((end_byte) == 0x30u) {
+ break;
+ }
+ // +0x30 | 0C 00 00 00 | UOffset32 | 0x0000000C (12) Loc: +0x3C | offset to field `data` (vector)
+ buffer = Push<flatbuffers::uoffset_t>(buffer, 0x0C);
+ // +0x34 | 69 00 00 00 | uint32_t | 0x00000069 (105) | table field `queue_index` (UInt)
+ buffer = Push<uint32_t>(buffer, context.remote_queue_index);
+ [[fallthrough]];
+ case 0x38u:
+ if ((end_byte) == 0x38u) {
+ break;
+ }
+ // +0x38 | F3 00 00 00 | uint32_t | 0x000000F3 (243) | table field `channel_index` (UInt)
+ buffer = Push<uint32_t>(buffer, channel_index);
+ //
+ // vector (aos.logger.MessageHeader.data):
+ // +0x3C | 1A 00 00 00 | uint32_t | 0x0000001A (26) | length of vector (# items)
+ buffer = Push<flatbuffers::uoffset_t>(buffer, context.size);
+ [[fallthrough]];
+ case 0x40u:
+ if ((end_byte) == 0x40u) {
+ break;
+ }
+ [[fallthrough]];
+ default:
+ // +0x40 | 38 | uint8_t | 0x38 (56) | value[0]
+ // +0x41 | 1A | uint8_t | 0x1A (26) | value[1]
+ // ...
+ // +0x58 | 90 | uint8_t | 0x90 (144) | value[24]
+ // +0x59 | 92 | uint8_t | 0x92 (146) | value[25]
+ //
+ // padding:
+ // +0x5A | 00 00 00 00 00 00 | uint8_t[6] | ...... | padding
+ // clang-format on
+ if (start_byte <= 0x40 && end_byte == message_size) {
+ // The easy one, slap it all down.
+ buffer = PushBytes(buffer, context.data, context.size);
+ buffer =
+ Pad(buffer, ((context.size + 7) & 0xfffffff8u) - context.size);
+ } else {
+ const size_t data_start_byte =
+ start_byte < 0x40 ? 0x0u : (start_byte - 0x40);
+ const size_t data_end_byte = end_byte - 0x40;
+ const size_t padded_size = ((context.size + 7) & 0xfffffff8u);
+ if (data_start_byte < padded_size) {
+ buffer = PushBytes(
+ buffer,
+ reinterpret_cast<const uint8_t *>(context.data) +
+ data_start_byte,
+ std::min(context.size, data_end_byte) - data_start_byte);
+ if (data_end_byte == padded_size) {
+ // We can only pad the last 7 bytes, so this only gets written
+ // if we write the last byte.
+ buffer = Pad(buffer,
+ ((context.size + 7) & 0xfffffff8u) - context.size);
+ }
+ }
+ }
+ break;
+ }
}
- return message_size;
+ return end_byte - start_byte;
}
SpanReader::SpanReader(std::string_view filename, bool quiet)
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index ca36ace..50f6b40 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -44,11 +44,30 @@
// 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,
std::unique_ptr<DataEncoder> encoder);
// Creates a dummy instance which won't even open a file. It will act as if
@@ -134,12 +153,13 @@
private:
// Performs a single writev call with as much of the data we have queued up as
- // possible.
+ // possible. now is the time we flushed at, to be recorded in
+ // last_flush_time_.
//
// This will normally take all of the data we have queued up, unless an
// encoder has spit out a big enough chunk all at once that we can't manage
// all of it.
- void Flush();
+ 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.
@@ -154,6 +174,21 @@
// 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<DataEncoder> encoder_;
@@ -172,6 +207,10 @@
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;
@@ -186,7 +225,8 @@
constexpr flatbuffers::uoffset_t PackRemoteMessageSize() { return 96u; }
size_t PackRemoteMessageInline(
uint8_t *data, const message_bridge::RemoteMessage *msg, int channel_index,
- const aos::monotonic_clock::time_point monotonic_timestamp_time);
+ const aos::monotonic_clock::time_point monotonic_timestamp_time,
+ size_t start_byte, size_t end_byte);
// Packes a message pointed to by the context into a MessageHeader.
flatbuffers::Offset<MessageHeader> PackMessage(
@@ -201,7 +241,8 @@
// This is equivalent to PackMessage, but doesn't require allocating a
// FlatBufferBuilder underneath.
size_t PackMessageInline(uint8_t *data, const Context &contex,
- int channel_index, LogType log_type);
+ int channel_index, LogType log_type, size_t start_byte,
+ size_t end_byte);
// Class to read chunks out of a log file.
class SpanReader {
@@ -912,6 +953,65 @@
// a single node.
std::string MaybeNodeName(const Node *);
+// Class to copy a RemoteMessage into the provided buffer.
+class RemoteMessageCopier : public DataEncoder::Copier {
+ public:
+ RemoteMessageCopier(const message_bridge::RemoteMessage *message,
+ int channel_index,
+ aos::monotonic_clock::time_point monotonic_timestamp_time,
+ EventLoop *event_loop)
+ : DataEncoder::Copier(PackRemoteMessageSize()),
+ message_(message),
+ channel_index_(channel_index),
+ monotonic_timestamp_time_(monotonic_timestamp_time),
+ event_loop_(event_loop) {}
+
+ monotonic_clock::time_point end_time() const { return end_time_; }
+
+ size_t Copy(uint8_t *data, size_t start_byte, size_t end_byte) final {
+ size_t result = PackRemoteMessageInline(data, message_, channel_index_,
+ monotonic_timestamp_time_,
+ start_byte, end_byte);
+ end_time_ = event_loop_->monotonic_now();
+ return result;
+ }
+
+ private:
+ const message_bridge::RemoteMessage *message_;
+ int channel_index_;
+ aos::monotonic_clock::time_point monotonic_timestamp_time_;
+ EventLoop *event_loop_;
+ monotonic_clock::time_point end_time_;
+};
+
+// Class to copy a context into the provided buffer.
+class ContextDataCopier : public DataEncoder::Copier {
+ public:
+ ContextDataCopier(const Context &context, int channel_index, LogType log_type,
+ EventLoop *event_loop)
+ : DataEncoder::Copier(PackMessageSize(log_type, context.size)),
+ context_(context),
+ channel_index_(channel_index),
+ log_type_(log_type),
+ event_loop_(event_loop) {}
+
+ monotonic_clock::time_point end_time() const { return end_time_; }
+
+ size_t Copy(uint8_t *data, size_t start_byte, size_t end_byte) final {
+ size_t result = PackMessageInline(data, context_, channel_index_, log_type_,
+ start_byte, end_byte);
+ end_time_ = event_loop_->monotonic_now();
+ return result;
+ }
+
+ private:
+ const Context &context_;
+ const int channel_index_;
+ const LogType log_type_;
+ EventLoop *event_loop_;
+ monotonic_clock::time_point end_time_;
+};
+
} // namespace aos::logger
#endif // AOS_EVENTS_LOGGING_LOGFILE_UTILS_H_
diff --git a/aos/events/logging/logfile_utils_test.cc b/aos/events/logging/logfile_utils_test.cc
index b3a9bbd..ec84b58 100644
--- a/aos/events/logging/logfile_utils_test.cc
+++ b/aos/events/logging/logfile_utils_test.cc
@@ -3100,6 +3100,90 @@
"/foo.afb");
}
+// Event loop which just has working time functions for the Copier classes
+// tested below.
+class TimeEventLoop : public EventLoop {
+ public:
+ TimeEventLoop() : EventLoop(nullptr) {}
+
+ aos::monotonic_clock::time_point monotonic_now() const final {
+ return aos::monotonic_clock::min_time;
+ }
+ realtime_clock::time_point realtime_now() const final {
+ return aos::realtime_clock::min_time;
+ }
+
+ void OnRun(::std::function<void()> /*on_run*/) final { LOG(FATAL); }
+
+ const std::string_view name() const final { return "time"; }
+ const Node *node() const final { return nullptr; }
+
+ void SetRuntimeAffinity(const cpu_set_t & /*cpuset*/) final { LOG(FATAL); }
+ void SetRuntimeRealtimePriority(int /*priority*/) final { LOG(FATAL); }
+
+ const cpu_set_t &runtime_affinity() const final {
+ LOG(FATAL);
+ return cpuset_;
+ }
+
+ TimerHandler *AddTimer(::std::function<void()> /*callback*/) final {
+ LOG(FATAL);
+ return nullptr;
+ }
+
+ std::unique_ptr<RawSender> MakeRawSender(const Channel * /*channel*/) final {
+ LOG(FATAL);
+ return std::unique_ptr<RawSender>();
+ }
+
+ const UUID &boot_uuid() const final {
+ LOG(FATAL);
+ return boot_uuid_;
+ }
+
+ void set_name(const std::string_view name) final { LOG(FATAL) << name; }
+
+ pid_t GetTid() final {
+ LOG(FATAL);
+ return 0;
+ }
+
+ int NumberBuffers(const Channel * /*channel*/) final {
+ LOG(FATAL);
+ return 0;
+ }
+
+ int runtime_realtime_priority() const final {
+ LOG(FATAL);
+ return 0;
+ }
+
+ std::unique_ptr<RawFetcher> MakeRawFetcher(
+ const Channel * /*channel*/) final {
+ LOG(FATAL);
+ return std::unique_ptr<RawFetcher>();
+ }
+
+ PhasedLoopHandler *AddPhasedLoop(
+ ::std::function<void(int)> /*callback*/,
+ const monotonic_clock::duration /*interval*/,
+ const monotonic_clock::duration /*offset*/) final {
+ LOG(FATAL);
+ return nullptr;
+ }
+
+ void MakeRawWatcher(
+ const Channel * /*channel*/,
+ std::function<void(const Context &context, const void *message)>
+ /*watcher*/) final {
+ LOG(FATAL);
+ }
+
+ private:
+ const cpu_set_t cpuset_ = DefaultAffinity();
+ UUID boot_uuid_ = UUID ::Zero();
+};
+
// Tests that all variations of PackMessage are equivalent to the inline
// PackMessage used to avoid allocations.
TEST_F(InlinePackMessage, Equivilent) {
@@ -3136,14 +3220,41 @@
67);
// And verify packing inline works as expected.
- EXPECT_EQ(repacked_message.size(),
- PackMessageInline(repacked_message.data(), context,
- channel_index, type));
+ EXPECT_EQ(
+ repacked_message.size(),
+ PackMessageInline(repacked_message.data(), context, channel_index,
+ type, 0u, repacked_message.size()));
EXPECT_EQ(absl::Span<uint8_t>(repacked_message),
absl::Span<uint8_t>(fbb.GetBufferSpan().data(),
fbb.GetBufferSpan().size()))
<< AnnotateBinaries(schema, "aos/events/logging/logger.bfbs",
fbb.GetBufferSpan());
+
+ // Ok, now we want to confirm that we can build up arbitrary pieces of
+ // said flatbuffer. Try all of them since it is cheap.
+ TimeEventLoop event_loop;
+ for (size_t i = 0; i < repacked_message.size(); i += 8) {
+ for (size_t j = i; j < repacked_message.size(); j += 8) {
+ std::vector<uint8_t> destination(repacked_message.size(), 67u);
+ ContextDataCopier copier(context, channel_index, type, &event_loop);
+
+ copier.Copy(destination.data(), i, j);
+
+ size_t index = 0;
+ for (size_t k = i; k < j; ++k) {
+ ASSERT_EQ(destination[index], repacked_message[k])
+ << ": Failed to match type " << static_cast<int>(type)
+ << ", index " << index << " while testing range " << i << " to "
+ << j;
+ ;
+ ++index;
+ }
+ // Now, confirm that none of the other bytes have been touched.
+ for (; index < destination.size(); ++index) {
+ ASSERT_EQ(destination[index], 67u);
+ }
+ }
+ }
}
}
}
@@ -3181,15 +3292,37 @@
std::vector<uint8_t> repacked_message(PackRemoteMessageSize(), 67);
// And verify packing inline works as expected.
- EXPECT_EQ(
- repacked_message.size(),
- PackRemoteMessageInline(repacked_message.data(), &random_msg.message(),
- channel_index, monotonic_timestamp_time));
+ EXPECT_EQ(repacked_message.size(),
+ PackRemoteMessageInline(
+ repacked_message.data(), &random_msg.message(), channel_index,
+ monotonic_timestamp_time, 0u, repacked_message.size()));
EXPECT_EQ(absl::Span<uint8_t>(repacked_message),
absl::Span<uint8_t>(fbb.GetBufferSpan().data(),
fbb.GetBufferSpan().size()))
<< AnnotateBinaries(schema, "aos/events/logging/logger.bfbs",
fbb.GetBufferSpan());
+
+ // Ok, now we want to confirm that we can build up arbitrary pieces of said
+ // flatbuffer. Try all of them since it is cheap.
+ TimeEventLoop event_loop;
+ for (size_t i = 0; i < repacked_message.size(); i += 8) {
+ for (size_t j = i; j < repacked_message.size(); j += 8) {
+ std::vector<uint8_t> destination(repacked_message.size(), 67u);
+ RemoteMessageCopier copier(&random_msg.message(), channel_index,
+ monotonic_timestamp_time, &event_loop);
+
+ copier.Copy(destination.data(), i, j);
+
+ size_t index = 0;
+ for (size_t k = i; k < j; ++k) {
+ ASSERT_EQ(destination[index], repacked_message[k]);
+ ++index;
+ }
+ for (; index < destination.size(); ++index) {
+ ASSERT_EQ(destination[index], 67u);
+ }
+ }
+ }
}
}
diff --git a/aos/events/logging/logger_main.cc b/aos/events/logging/logger_main.cc
index 81b4e3f..f0582dc 100644
--- a/aos/events/logging/logger_main.cc
+++ b/aos/events/logging/logger_main.cc
@@ -32,6 +32,8 @@
DEFINE_int32(xz_compression_level, 9, "Compression level for the LZMA Encoder");
#endif
+DECLARE_int32(flush_size);
+
int main(int argc, char *argv[]) {
gflags::SetUsageMessage(
"This program provides a simple logger binary that logs all SHMEM data "
@@ -53,14 +55,15 @@
if (FLAGS_snappy_compress) {
log_namer->set_extension(aos::logger::SnappyDecoder::kExtension);
log_namer->set_encoder_factory([](size_t max_message_size) {
- return std::make_unique<aos::logger::SnappyEncoder>(max_message_size);
+ return std::make_unique<aos::logger::SnappyEncoder>(max_message_size,
+ FLAGS_flush_size);
});
#ifdef LZMA
} else if (FLAGS_xz_compress) {
log_namer->set_extension(aos::logger::LzmaEncoder::kExtension);
log_namer->set_encoder_factory([](size_t max_message_size) {
return std::make_unique<aos::logger::LzmaEncoder>(
- max_message_size, FLAGS_xz_compression_level);
+ max_message_size, FLAGS_xz_compression_level, FLAGS_flush_size);
});
#endif
}
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 18c8aa1..cdf080b 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -557,7 +557,7 @@
}},
{SnappyDecoder::kExtension,
[](size_t max_message_size) {
- return std::make_unique<SnappyEncoder>(max_message_size);
+ return std::make_unique<SnappyEncoder>(max_message_size, 32768);
}},
#ifdef LZMA
{LzmaDecoder::kExtension,
diff --git a/aos/events/logging/lzma_encoder.cc b/aos/events/logging/lzma_encoder.cc
index f583818..f354638 100644
--- a/aos/events/logging/lzma_encoder.cc
+++ b/aos/events/logging/lzma_encoder.cc
@@ -88,19 +88,22 @@
LzmaEncoder::~LzmaEncoder() { lzma_end(&stream_); }
-void LzmaEncoder::Encode(Copier *copy) {
+size_t LzmaEncoder::Encode(Copier *copy, size_t start_byte) {
const size_t copy_size = copy->size();
// LZMA compresses the data as it goes along, copying the compressed results
// into another buffer. So, there's no need to store more than one message
// since lzma is going to take it from here.
CHECK_LE(copy_size, input_buffer_.size());
- CHECK_EQ(copy->Copy(input_buffer_.data()), copy_size);
+ CHECK_EQ(copy->Copy(input_buffer_.data(), start_byte, copy_size - start_byte),
+ copy_size - start_byte);
stream_.next_in = input_buffer_.data();
stream_.avail_in = copy_size;
RunLzmaCode(LZMA_RUN);
+
+ return copy_size - start_byte;
}
void LzmaEncoder::Finish() { RunLzmaCode(LZMA_FINISH); }
diff --git a/aos/events/logging/lzma_encoder.h b/aos/events/logging/lzma_encoder.h
index bbd739a..b4964fb 100644
--- a/aos/events/logging/lzma_encoder.h
+++ b/aos/events/logging/lzma_encoder.h
@@ -37,7 +37,8 @@
// space.
return true;
}
- void Encode(Copier *copy) final;
+ size_t space() const final { return input_buffer_.capacity(); }
+ size_t Encode(Copier *copy, size_t start_byte) final;
void Finish() final;
void Clear(int n) final;
absl::Span<const absl::Span<const uint8_t>> queue() final;
diff --git a/aos/events/logging/snappy_encoder.cc b/aos/events/logging/snappy_encoder.cc
index 2ef8363..0e96092 100644
--- a/aos/events/logging/snappy_encoder.cc
+++ b/aos/events/logging/snappy_encoder.cc
@@ -43,12 +43,15 @@
void SnappyEncoder::Finish() { EncodeCurrentBuffer(); }
-void SnappyEncoder::Encode(Copier *copy) {
+size_t SnappyEncoder::Encode(Copier *copy, size_t start_byte) {
+ CHECK_EQ(start_byte, 0u);
buffer_source_.Append(copy);
if (buffer_source_.Available() >= chunk_size_) {
EncodeCurrentBuffer();
}
+
+ return copy->size();
}
void SnappyEncoder::EncodeCurrentBuffer() {
@@ -142,7 +145,7 @@
CHECK_LE(copy_size + data_.size(), data_.capacity());
size_t starting_size = data_.size();
data_.resize(starting_size + copy_size);
- CHECK_EQ(copy->Copy(data_.data() + starting_size), copy_size);
+ CHECK_EQ(copy->Copy(data_.data() + starting_size, 0, copy_size), copy_size);
accumulated_checksum_ = AccumulateCrc32(
{data_.data() + starting_size, copy_size}, accumulated_checksum_);
}
diff --git a/aos/events/logging/snappy_encoder.h b/aos/events/logging/snappy_encoder.h
index d3edbe5..d698ee1 100644
--- a/aos/events/logging/snappy_encoder.h
+++ b/aos/events/logging/snappy_encoder.h
@@ -16,9 +16,9 @@
// Encodes buffers using snappy.
class SnappyEncoder final : public DataEncoder {
public:
- explicit SnappyEncoder(size_t max_message_size, size_t chunk_size = 32768);
+ explicit SnappyEncoder(size_t max_message_size, size_t chunk_size = 128 * 1024);
- void Encode(Copier *copy) final;
+ size_t Encode(Copier *copy, size_t start_byte) final;
void Finish() final;
void Clear(int n) final;
@@ -28,6 +28,7 @@
// Since the output always mallocs space, we have infinite output space.
return true;
}
+ size_t space() const final { return buffer_source_.space(); }
size_t total_bytes() const final { return total_bytes_; }
size_t queue_size() const final { return queue_.size(); }
@@ -35,6 +36,7 @@
class DetachedBufferSource : public snappy::Source {
public:
DetachedBufferSource(size_t buffer_size);
+ size_t space() const { return data_.capacity() - data_.size(); }
size_t Available() const final;
const char *Peek(size_t *length) final;
void Skip(size_t n) final;
diff --git a/aos/util/foxglove_websocket.cc b/aos/util/foxglove_websocket.cc
index 26092bc..6ecb600 100644
--- a/aos/util/foxglove_websocket.cc
+++ b/aos/util/foxglove_websocket.cc
@@ -5,6 +5,12 @@
DEFINE_string(config, "/app/aos_config.json", "Path to the config.");
DEFINE_uint32(port, 8765, "Port to use for foxglove websocket server.");
+DEFINE_string(mode, "flatbuffer", "json or flatbuffer serialization.");
+DEFINE_bool(fetch_pinned_channels, true,
+ "Set this to allow foxglove_websocket to make fetchers on channels "
+ "with a read_method of PIN (see aos/configuration.fbs; PIN is an "
+ "enum value). Having this enabled will cause foxglove to consume "
+ "extra shared memory resources.");
int main(int argc, char *argv[]) {
gflags::SetUsageMessage(
@@ -40,7 +46,14 @@
aos::ShmEventLoop event_loop(&config.message());
- aos::FoxgloveWebsocketServer server(&event_loop, FLAGS_port);
+ aos::FoxgloveWebsocketServer server(
+ &event_loop, FLAGS_port,
+ FLAGS_mode == "flatbuffer"
+ ? aos::FoxgloveWebsocketServer::Serialization::kFlatbuffer
+ : aos::FoxgloveWebsocketServer::Serialization::kJson,
+ FLAGS_fetch_pinned_channels
+ ? aos::FoxgloveWebsocketServer::FetchPinnedChannels::kYes
+ : aos::FoxgloveWebsocketServer::FetchPinnedChannels::kNo);
event_loop.Run();
}
diff --git a/aos/util/foxglove_websocket_lib.cc b/aos/util/foxglove_websocket_lib.cc
index b4d262a..1cc3a8a 100644
--- a/aos/util/foxglove_websocket_lib.cc
+++ b/aos/util/foxglove_websocket_lib.cc
@@ -1,17 +1,13 @@
#include "aos/util/foxglove_websocket_lib.h"
#include "aos/util/mcap_logger.h"
+#include "aos/flatbuffer_merge.h"
+#include "absl/strings/escaping.h"
#include "gflags/gflags.h"
DEFINE_uint32(sorting_buffer_ms, 100,
"Amount of time to buffer messages to sort them before sending "
"them to foxglove.");
-DEFINE_bool(fetch_pinned_channels, false,
- "Set this to allow foxglove_websocket to make fetchers on channels "
- "with a read_method of PIN (see aos/configuration.fbs; PIN is an "
- "enum value). By default, we don't make fetchers for "
- "these channels since using up a fetcher slot on PIN'd channels "
- "can have side-effects.");
namespace {
// Period at which to poll the fetchers for all the channels.
@@ -19,21 +15,38 @@
} // namespace
namespace aos {
-FoxgloveWebsocketServer::FoxgloveWebsocketServer(aos::EventLoop *event_loop,
- uint32_t port)
- : event_loop_(event_loop), server_(port, "aos_foxglove") {
+FoxgloveWebsocketServer::FoxgloveWebsocketServer(
+ aos::EventLoop *event_loop, uint32_t port, Serialization serialization,
+ FetchPinnedChannels fetch_pinned_channels)
+ : event_loop_(event_loop),
+ serialization_(serialization),
+ fetch_pinned_channels_(fetch_pinned_channels),
+ server_(port, "aos_foxglove") {
for (const aos::Channel *channel :
*event_loop_->configuration()->channels()) {
const bool is_pinned = (channel->read_method() == ReadMethod::PIN);
if (aos::configuration::ChannelIsReadableOnNode(channel,
event_loop_->node()) &&
- (!is_pinned || FLAGS_fetch_pinned_channels)) {
+ (!is_pinned || fetch_pinned_channels_ == FetchPinnedChannels::kYes)) {
+ const FlatbufferDetachedBuffer<reflection::Schema> schema =
+ RecursiveCopyFlatBuffer(channel->schema());
const ChannelId id =
- server_.addChannel(foxglove::websocket::ChannelWithoutId{
- .topic = channel->name()->str() + " " + channel->type()->str(),
- .encoding = "json",
- .schemaName = channel->type()->str(),
- .schema = JsonSchemaForFlatbuffer({channel->schema()}).dump()});
+ (serialization_ == Serialization::kJson)
+ ? server_.addChannel(foxglove::websocket::ChannelWithoutId{
+ .topic =
+ channel->name()->str() + " " + channel->type()->str(),
+ .encoding = "json",
+ .schemaName = channel->type()->str(),
+ .schema =
+ JsonSchemaForFlatbuffer({channel->schema()}).dump()})
+ : server_.addChannel(foxglove::websocket::ChannelWithoutId{
+ .topic =
+ channel->name()->str() + " " + channel->type()->str(),
+ .encoding = "flatbuffer",
+ .schemaName = channel->type()->str(),
+ .schema = absl::Base64Escape(
+ {reinterpret_cast<const char *>(schema.span().data()),
+ schema.span().size()})});
CHECK(fetchers_.count(id) == 0);
fetchers_[id] =
FetcherState{.fetcher = event_loop_->MakeRawFetcher(channel)};
@@ -100,11 +113,20 @@
while (!fetcher_times.empty()) {
const ChannelId channel = fetcher_times.begin()->second;
FetcherState *fetcher = &fetchers_[channel];
- server_.sendMessage(
- channel, fetcher_times.begin()->first.time_since_epoch().count(),
- aos::FlatbufferToJson(
- fetcher->fetcher->channel()->schema(),
- static_cast<const uint8_t *>(fetcher->fetcher->context().data)));
+ switch (serialization_) {
+ case Serialization::kJson:
+ server_.sendMessage(
+ channel, fetcher_times.begin()->first.time_since_epoch().count(),
+ aos::FlatbufferToJson(fetcher->fetcher->channel()->schema(),
+ static_cast<const uint8_t *>(
+ fetcher->fetcher->context().data)));
+ break;
+ case Serialization::kFlatbuffer:
+ server_.sendMessage(
+ channel, fetcher_times.begin()->first.time_since_epoch().count(),
+ {static_cast<const char *>(fetcher->fetcher->context().data),
+ fetcher->fetcher->context().size});
+ }
fetcher_times.erase(fetcher_times.begin());
fetcher->sent_current_message = true;
if (fetcher->fetcher->FetchNext()) {
diff --git a/aos/util/foxglove_websocket_lib.h b/aos/util/foxglove_websocket_lib.h
index 8160653..9be2f61 100644
--- a/aos/util/foxglove_websocket_lib.h
+++ b/aos/util/foxglove_websocket_lib.h
@@ -14,7 +14,19 @@
// See foxglove_websocket.cc for some usage notes.
class FoxgloveWebsocketServer {
public:
- FoxgloveWebsocketServer(aos::EventLoop *event_loop, uint32_t port);
+ // Whether to serialize the messages into the MCAP file as JSON or
+ // flatbuffers.
+ enum class Serialization {
+ kJson,
+ kFlatbuffer,
+ };
+ enum class FetchPinnedChannels {
+ kYes,
+ kNo,
+ };
+ FoxgloveWebsocketServer(aos::EventLoop *event_loop, uint32_t port,
+ Serialization serialization,
+ FetchPinnedChannels fetch_pinned_channels);
~FoxgloveWebsocketServer();
private:
@@ -33,6 +45,8 @@
};
aos::EventLoop *event_loop_;
+ const Serialization serialization_;
+ const FetchPinnedChannels fetch_pinned_channels_;
foxglove::websocket::Server server_;
// A map of fetchers for every single channel that could be subscribed to.
std::map<ChannelId, FetcherState> fetchers_;
diff --git a/aos/util/mcap_logger.cc b/aos/util/mcap_logger.cc
index 96a9b60..40e55f0 100644
--- a/aos/util/mcap_logger.cc
+++ b/aos/util/mcap_logger.cc
@@ -309,7 +309,7 @@
CHECK(channel->has_schema());
const FlatbufferDetachedBuffer<reflection::Schema> schema =
- CopyFlatBuffer(channel->schema());
+ RecursiveCopyFlatBuffer(channel->schema());
// Write out the schema (we don't bother deduplicating schema types):
string_builder_.Reset();
diff --git a/documentation/README.md b/documentation/README.md
index 32b8eb1..0f6485c 100644
--- a/documentation/README.md
+++ b/documentation/README.md
@@ -13,3 +13,4 @@
* [Create a new autonomous routine](tutorials/create-a-new-autonomous.md)
* [Tune an autonomous](tutorials/tune-an-autonomous.md)
* [Set up access to the build server using vscode](tutorials/setup-ssh-vscode.md)
+* [Install PyCharm on the build server](tutorials/setup-pycharm-on-build-server.md)
diff --git a/documentation/tutorials/setup-pycharm-on-build-server.md b/documentation/tutorials/setup-pycharm-on-build-server.md
new file mode 100644
index 0000000..acbb66d
--- /dev/null
+++ b/documentation/tutorials/setup-pycharm-on-build-server.md
@@ -0,0 +1,20 @@
+## Installing PyCharm on the build server
+
+### Getting PyCharm Professional (optional)
+Go to JetBrains' student [website](https://www.jetbrains.com/community/education/#students) and click "Apply now". Fill out the form using your school email. Check your school email for an email from JetBrains. Create a new JetBrains account. Use this account to log in when PyCharn asks for a professional key
+
+### Downloading on the build server
+Open your shell and run
+```
+curl https://raw.githubusercontent.com/thealtofwar/files/main/install_pycharm.sh > ~/install_pycharm.sh
+chmod +x ~/install_pycharm.sh
+~/install_pycharm.sh
+```
+This installs PyCharm to your desktop.
+### Alternate download method
+When you're on the build server, go to the [download](https://www.jetbrains.com/pycharm/download/#section=linux) and pick which edition of PyCharm you would like to download. Once the download completes, click on it to extract it. When something that looks like this appears,<br>
+<br>
+Look for the button that looks like a up arrow with a line of top that's labeled root that looks like [this](https://raw.githubusercontent.com/thealtofwar/files/main/Screenshot%202021-11-11%20101425.png). Click on it. Drag the folder to your desktop. It will say an error occured, but it should be fine.
+
+### Installing PyCharm on the build server
+When you're on the build server, go to the folder that you just downloaded PyCharm to. Click on the 'bin' folder. Click on the 'pycharm.sh' file. PyCharm should start running. If you downloaded the Professional version, you might have to log in with the JetBrains account you made when signing up. Open the folder where your project is located. Now you can start coding.
diff --git a/frc971/constants/BUILD b/frc971/constants/BUILD
index 891067c..c17dc31 100644
--- a/frc971/constants/BUILD
+++ b/frc971/constants/BUILD
@@ -4,10 +4,12 @@
"constants_sender_lib.h",
],
target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
deps = [
"//aos:flatbuffer_merge",
"//aos:json_to_flatbuffer",
"//aos/events:event_loop",
+ "//aos/events:shm_event_loop",
"//aos/network:team_number",
"@com_github_gflags_gflags//:gflags",
"@com_github_google_glog//:glog",
diff --git a/frc971/constants/constants_sender_lib.h b/frc971/constants/constants_sender_lib.h
index 92c14fa..48cacb5 100644
--- a/frc971/constants/constants_sender_lib.h
+++ b/frc971/constants/constants_sender_lib.h
@@ -2,6 +2,7 @@
#define FRC971_CONSTANTS_CONSTANTS_SENDER_H_
#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/network/team_number.h"
@@ -27,11 +28,9 @@
constants_path_(constants_path),
event_loop_(event_loop),
sender_(event_loop_->MakeSender<ConstantsData>(channel_name_)) {
- event_loop->OnRun([this]() {
- typename aos::Sender<ConstantsData>::Builder builder =
- sender_.MakeBuilder();
- builder.CheckOk(builder.Send(GetConstantsForTeamNumber(builder.fbb())));
- });
+ typename aos::Sender<ConstantsData>::Builder builder =
+ sender_.MakeBuilder();
+ builder.CheckOk(builder.Send(GetConstantsForTeamNumber(builder.fbb())));
}
private:
@@ -63,6 +62,56 @@
aos::Sender<ConstantsData> sender_;
};
+// This class fetches the current constants for the device, with appropriate
+// CHECKs to ensure that (a) the constants never change and (b) that the
+// constants are always available. This can be paired with WaitForConstants to
+// create the conditions for (b). In simulation, the constants should simply be
+// sent before starting up other EventLoops.
+template <typename ConstantsData>
+class ConstantsFetcher {
+ public:
+ ConstantsFetcher(aos::EventLoop *event_loop,
+ std::string_view channel = "/constants")
+ : fetcher_(event_loop->MakeFetcher<ConstantsData>(channel)) {
+ CHECK(fetcher_.Fetch())
+ << "Constants information must be available at startup.";
+ event_loop->MakeNoArgWatcher<ConstantsData>(channel, []() {
+ LOG(FATAL)
+ << "Don't know how to handle changes to constants information.";
+ });
+ }
+
+ const ConstantsData& constants() const {
+ return *fetcher_.get();
+ }
+
+ private:
+ aos::Fetcher<ConstantsData> fetcher_;
+};
+
+// Blocks until data is available on the requested channel using a ShmEventLoop.
+// This is for use during initialization in C++ binaries so that we can delay
+// initialization until everything is available. This allows applications to
+// depend on constants data during their initialization.
+template <typename ConstantsData>
+void WaitForConstants(const aos::Configuration *config,
+ std::string_view channel = "/constants") {
+ aos::ShmEventLoop event_loop(config);
+ aos::Fetcher fetcher = event_loop.MakeFetcher<ConstantsData>(channel);
+ event_loop.MakeNoArgWatcher<ConstantsData>(
+ channel, [&event_loop]() { event_loop.Exit(); });
+ event_loop.OnRun([&event_loop, &fetcher]() {
+ // If the constants were already published, we don't need to wait for them.
+ if (fetcher.Fetch()) {
+ event_loop.Exit();
+ }
+ });
+ LOG(INFO) << "Waiting for constants data on " << channel << " "
+ << ConstantsData::GetFullyQualifiedName();
+ event_loop.Run();
+ LOG(INFO) << "Got constants data.";
+}
+
} // namespace frc971::constants
#endif // FRC971_CONSTANTS_CONSTANTS_SENDER_H_
diff --git a/frc971/constants/constants_sender_test.cc b/frc971/constants/constants_sender_test.cc
index 3edc320..1441767 100644
--- a/frc971/constants/constants_sender_test.cc
+++ b/frc971/constants/constants_sender_test.cc
@@ -40,11 +40,10 @@
ConstantSender<testdata::ConstantsData, testdata::ConstantsList> test971(
constants_sender_event_loop_.get(),
"frc971/constants/testdata/test_constants.json", "/constants");
- test_event_loop->MakeWatcher("/constants",
- [](const testdata::ConstantsData &data) {
- EXPECT_EQ(data.max_roller_voltage(), 12);
- EXPECT_EQ(data.min_roller_voltage(), -12);
- });
+ ConstantsFetcher<testdata::ConstantsData> fetcher(test_event_loop.get());
+ EXPECT_EQ(fetcher.constants().max_roller_voltage(), 12);
+ EXPECT_EQ(fetcher.constants().min_roller_voltage(), -12);
+ // Ensure that the watcher in ConstantsFetcher never triggers.
event_loop_factory_.RunFor(std::chrono::seconds(1));
}
@@ -57,14 +56,42 @@
ConstantSender<testdata::ConstantsData, testdata::ConstantsList> test971(
constants_sender_event_loop_.get(),
"frc971/constants/testdata/test_constants.json", 9971, "/constants");
- test_event_loop->MakeWatcher("/constants",
- [](const testdata::ConstantsData &data) {
- EXPECT_EQ(data.max_roller_voltage(), 6);
- EXPECT_EQ(data.min_roller_voltage(), -6);
- });
+ ConstantsFetcher<testdata::ConstantsData> fetcher(test_event_loop.get());
+ EXPECT_EQ(fetcher.constants().max_roller_voltage(), 6);
+ EXPECT_EQ(fetcher.constants().min_roller_voltage(), -6);
event_loop_factory_.RunFor(std::chrono::seconds(1));
}
+// Tests that the ConstantsFetcher dies when there is no data available during
+// construction.
+TEST_F(ConstantSenderTest, NoDataOnStartup) {
+ std::unique_ptr<aos::EventLoop> test_event_loop =
+ event_loop_factory_.MakeEventLoop("constants");
+ EXPECT_DEATH(ConstantsFetcher<testdata::ConstantsData>(test_event_loop.get()),
+ "information must be available at startup");
+}
+
+// Tests that the ConstantsFetcher dies when there is a change to the constants
+// data.
+TEST_F(ConstantSenderTest, DieOnDataUpdate) {
+ std::unique_ptr<aos::EventLoop> test_event_loop =
+ event_loop_factory_.MakeEventLoop("constants");
+ ConstantSender<testdata::ConstantsData, testdata::ConstantsList> test971(
+ constants_sender_event_loop_.get(),
+ "frc971/constants/testdata/test_constants.json", 9971, "/constants");
+ ConstantsFetcher<testdata::ConstantsData> fetcher(test_event_loop.get());
+ auto sender =
+ constants_sender_event_loop_->MakeSender<testdata::ConstantsData>(
+ "/constants");
+ constants_sender_event_loop_->OnRun([&sender]() {
+ auto builder = sender.MakeBuilder();
+ builder.CheckOk(builder.Send(
+ builder.MakeBuilder<testdata::ConstantsData>().Finish()));
+ });
+ EXPECT_DEATH(event_loop_factory_.RunFor(std::chrono::seconds(1)),
+ "changes to constants");
+}
+
// When given a team number that it not recognized we kill the program.
TEST_F(ConstantSenderTest, TeamNotFound) {
diff --git a/frc971/control_loops/drivetrain/distance_spline.h b/frc971/control_loops/drivetrain/distance_spline.h
index 91b9898..f445fa4 100644
--- a/frc971/control_loops/drivetrain/distance_spline.h
+++ b/frc971/control_loops/drivetrain/distance_spline.h
@@ -123,7 +123,7 @@
// The spline we are converting to a distance.
aos::SizedArray<Spline, kMaxSplines> splines_;
// An interpolation table of distances evenly distributed in alpha.
- const absl::Span<const float> distances_;
+ absl::Span<const float> distances_;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index a06b965..0d100ed 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -146,9 +146,10 @@
class DrivetrainLoop
: public frc971::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- // Note that we only actually store N - 1 splines, since we need to keep one
- // fetcher free to check whether there are any new splines.
- static constexpr size_t kNumSplineFetchers = 5;
+ // Note that we only actually store N - 1 splines consistently, since we need
+ // to keep one fetcher free to check whether there are any new splines.
+ static constexpr size_t kNumSplineFetchers =
+ SplineDrivetrain::kMaxTrajectories;
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 5a4e5da..0f9418c 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -36,18 +36,16 @@
void SplineDrivetrain::SetGoal(
const ::frc971::control_loops::drivetrain::Goal *goal) {
- if (goal->has_spline_handle()) {
- commanded_spline_ = goal->spline_handle();
- } else {
- commanded_spline_.reset();
- }
- UpdateSplineHandles();
+ UpdateSplineHandles(goal->has_spline_handle()
+ ? std::make_optional<int>(goal->spline_handle())
+ : std::nullopt);
}
bool SplineDrivetrain::IsCurrentTrajectory(
const fb::Trajectory *trajectory) const {
- return (current_trajectory_ != nullptr &&
- current_trajectory().spline_handle() == trajectory->handle());
+ const FinishedTrajectory *current = current_trajectory();
+ return (current != nullptr &&
+ current->spline_handle() == trajectory->handle());
}
bool SplineDrivetrain::HasTrajectory(const fb::Trajectory *trajectory) const {
@@ -55,7 +53,7 @@
return false;
}
for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
- if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+ if (trajectories_[ii].spline_handle() == trajectory->handle()) {
return true;
}
}
@@ -66,7 +64,7 @@
CHECK(trajectory != nullptr);
for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
- if (trajectories_[ii]->spline_handle() == trajectory->handle()) {
+ if (trajectories_[ii].spline_handle() == trajectory->handle()) {
trajectories_.erase(trajectories_.begin() + ii);
return;
}
@@ -76,49 +74,69 @@
}
void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
- trajectories_.emplace_back(std::make_unique<FinishedTrajectory>(
- dt_config_, trajectory, velocity_drivetrain_));
- UpdateSplineHandles();
+ CHECK_LT(trajectories_.size(), trajectories_.capacity());
+ trajectories_.emplace_back(dt_config_, trajectory, velocity_drivetrain_);
+ UpdateSplineHandles(commanded_spline_);
}
void SplineDrivetrain::DeleteCurrentSpline() {
- DeleteTrajectory(&CHECK_NOTNULL(current_trajectory_)->trajectory());
+ DeleteTrajectory(&CHECK_NOTNULL(current_trajectory())->trajectory());
executing_spline_ = false;
- current_trajectory_ = nullptr;
+ commanded_spline_.reset();
current_xva_.setZero();
}
-void SplineDrivetrain::UpdateSplineHandles() {
+void SplineDrivetrain::UpdateSplineHandles(
+ std::optional<int> commanded_spline) {
// If we are currently executing a spline and have received a change
if (executing_spline_) {
- if (!commanded_spline_) {
+ if (!commanded_spline) {
// We've been told to stop executing a spline; remove it from our queue,
// and clean up.
DeleteCurrentSpline();
return;
} else {
if (executing_spline_ &&
- current_trajectory().spline_handle() != *commanded_spline_) {
+ CHECK_NOTNULL(current_trajectory())->spline_handle() !=
+ *commanded_spline) {
// If we are executing a spline, and the handle has changed, garbage
// collect the old spline.
DeleteCurrentSpline();
}
}
}
+ commanded_spline_ = commanded_spline;
// We've now cleaned up the previous state; handle any new commands.
if (!commanded_spline_) {
return;
}
for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
- if (trajectories_[ii]->spline_handle() == *commanded_spline_) {
+ if (trajectories_[ii].spline_handle() == *commanded_spline_) {
executing_spline_ = true;
- current_trajectory_ = trajectories_[ii].get();
}
}
// If we didn't find the commanded spline in the list of available splines,
// that's fine; it just means, it hasn't been fully planned yet.
}
+FinishedTrajectory *SplineDrivetrain::current_trajectory() {
+ for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+ if (trajectories_[ii].spline_handle() == *commanded_spline_) {
+ return &trajectories_[ii];
+ }
+ }
+ return nullptr;
+}
+
+const FinishedTrajectory *SplineDrivetrain::current_trajectory() const {
+ for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
+ if (trajectories_[ii].spline_handle() == *commanded_spline_) {
+ return &trajectories_[ii];
+ }
+ }
+ return nullptr;
+}
+
// TODO(alex): Hold position when done following the spline.
void SplineDrivetrain::Update(
bool enable, const ::Eigen::Matrix<double, 5, 1> &state,
@@ -127,17 +145,19 @@
enable_ = enable;
if (enable && executing_spline_) {
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
+ const FinishedTrajectory *const trajectory =
+ CHECK_NOTNULL(current_trajectory());
if (!IsAtEnd() && executing_spline_) {
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
- U_ff = current_trajectory().FFVoltage(current_xva_(0));
+ U_ff = trajectory->FFVoltage(current_xva_(0));
}
const double current_distance = current_xva_(0);
::Eigen::Matrix<double, 2, 5> K =
- current_trajectory().GainForDistance(current_distance);
+ trajectory->GainForDistance(current_distance);
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
- const bool backwards = current_trajectory().drive_spline_backwards();
+ const bool backwards = trajectory->drive_spline_backwards();
if (backwards) {
::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
U_ff = -swapU;
@@ -148,11 +168,11 @@
goal_state(4, 0) = -left_goal;
}
const Eigen::Matrix<double, 5, 1> relative_goal =
- current_trajectory().StateToPathRelativeState(current_distance,
- goal_state, backwards);
+ trajectory->StateToPathRelativeState(current_distance, goal_state,
+ backwards);
const Eigen::Matrix<double, 5, 1> relative_state =
- current_trajectory().StateToPathRelativeState(current_distance, state,
- backwards);
+ trajectory->StateToPathRelativeState(current_distance, state,
+ backwards);
Eigen::Matrix<double, 5, 1> state_error = relative_goal - relative_state;
state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
@@ -163,7 +183,7 @@
}
::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2, 1>(0, 0);
- next_xva_ = current_trajectory().GetNextXVA(dt_config_.dt, &xv_state);
+ next_xva_ = trajectory->GetNextXVA(dt_config_.dt, &xv_state);
next_U_ = U_ff + U_fb - voltage_error;
uncapped_U_ = next_U_;
ScaleCapU(&next_U_);
@@ -203,7 +223,7 @@
builder->CreateUninitializedVector(trajectories_.size(), &spline_handles);
for (size_t ii = 0; ii < trajectories_.size(); ++ii) {
- spline_handles[ii] = trajectories_[ii]->spline_handle();
+ spline_handles[ii] = trajectories_[ii].spline_handle();
}
drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
@@ -211,7 +231,7 @@
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
trajectory_logging_builder.add_x(goal_state(0));
trajectory_logging_builder.add_y(goal_state(1));
- if (current_trajectory().drive_spline_backwards()) {
+ if (CHECK_NOTNULL(current_trajectory())->drive_spline_backwards()) {
trajectory_logging_builder.add_left_velocity(-goal_state(4));
trajectory_logging_builder.add_right_velocity(-goal_state(3));
trajectory_logging_builder.add_theta(
@@ -223,8 +243,7 @@
trajectory_logging_builder.add_right_velocity(goal_state(4));
}
}
- trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
- executing_spline_);
+ trajectory_logging_builder.add_is_executing(!IsAtEnd() && executing_spline_);
trajectory_logging_builder.add_is_executed(executing_spline_ && IsAtEnd());
if (commanded_spline_) {
trajectory_logging_builder.add_goal_spline_handle(*commanded_spline_);
@@ -233,8 +252,9 @@
}
}
trajectory_logging_builder.add_distance_remaining(
- executing_spline_ ? current_trajectory().length() - current_xva_.x()
- : 0.0);
+ executing_spline_
+ ? CHECK_NOTNULL(current_trajectory())->length() - current_xva_.x()
+ : 0.0);
trajectory_logging_builder.add_available_splines(handles_vector);
return trajectory_logging_builder.Finish();
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 390236b..cd712bc 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -5,7 +5,6 @@
#include <thread>
#include "Eigen/Dense"
-
#include "aos/condition.h"
#include "aos/mutex/mutex.h"
#include "frc971/control_loops/control_loops_generated.h"
@@ -23,6 +22,7 @@
class SplineDrivetrain {
public:
+ static constexpr size_t kMaxTrajectories = 5;
SplineDrivetrain(const DrivetrainConfig<double> &dt_config);
void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
@@ -49,15 +49,15 @@
// Accessor for the current goal state, pretty much only present for debugging
// purposes.
::Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
- return executing_spline_ ? current_trajectory().GoalState(current_xva_(0),
- current_xva_(1))
+ return executing_spline_ ? CHECK_NOTNULL(current_trajectory())
+ ->GoalState(current_xva_(0), current_xva_(1))
: ::Eigen::Matrix<double, 5, 1>::Zero();
}
bool IsAtEnd() const {
- return executing_spline_
- ? current_trajectory().is_at_end(current_xva_.block<2, 1>(0, 0))
- : true;
+ return executing_spline_ ? CHECK_NOTNULL(current_trajectory())
+ ->is_at_end(current_xva_.block<2, 1>(0, 0))
+ : true;
}
size_t trajectory_count() const { return trajectories_.size(); }
@@ -70,21 +70,20 @@
// This is called to update the internal state for managing all the splines.
// Calling it redundantly does not cause any issues. It checks the value of
- // commanded_spline_ to determine whether we are being commanded to run a
+ // commanded_spline to determine whether we are being commanded to run a
// spline, and if there is any trajectory in the list of trajectories matching
- // the command, we begin/continue executing that spline. If commanded_spline_
+ // the command, we begin/continue executing that spline. If commanded_spline
// is empty or has changed, we stop executing the previous trajectory and
// remove it from trajectories_. Then, when the drivetrain code checks
// HasTrajectory() for the old trajectory, it will return false and the
// drivetrain can free up the fetcher to get the next trajectory.
- void UpdateSplineHandles();
+ void UpdateSplineHandles(std::optional<int> commanded_spline);
// Deletes the currently executing trajectory.
void DeleteCurrentSpline();
- const FinishedTrajectory ¤t_trajectory() const {
- return *CHECK_NOTNULL(current_trajectory_);
- }
+ FinishedTrajectory *current_trajectory();
+ const FinishedTrajectory *current_trajectory() const;
const DrivetrainConfig<double> dt_config_;
@@ -97,8 +96,7 @@
// TODO(james): Sort out construction to avoid so much dynamic memory
// allocation...
- std::vector<std::unique_ptr<FinishedTrajectory>> trajectories_;
- const FinishedTrajectory *current_trajectory_ = nullptr;
+ aos::SizedArray<FinishedTrajectory, kMaxTrajectories> trajectories_;
std::optional<int> commanded_spline_;
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 6bccc74..ab1c55f 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -3,12 +3,12 @@
#include <chrono>
-#include "aos/flatbuffers.h"
#include "Eigen/Dense"
+#include "aos/flatbuffers.h"
#include "frc971/control_loops/drivetrain/distance_spline.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "frc971/control_loops/drivetrain/trajectory_generated.h"
#include "frc971/control_loops/drivetrain/spline_goal_generated.h"
+#include "frc971/control_loops/drivetrain/trajectory_generated.h"
#include "frc971/control_loops/hybrid_state_feedback_loop.h"
#include "frc971/control_loops/runge_kutta.h"
#include "frc971/control_loops/state_feedback_loop.h"
@@ -193,11 +193,11 @@
HybridKalman<2, 2, 2>>>
velocity_drivetrain_;
- const DrivetrainConfig<double> config_;
+ DrivetrainConfig<double> config_;
// Robot radiuses.
- const double robot_radius_l_;
- const double robot_radius_r_;
+ double robot_radius_l_;
+ double robot_radius_r_;
float lateral_acceleration_ = 3.0;
float longitudinal_acceleration_ = 2.0;
float voltage_limit_ = 12.0;
@@ -205,7 +205,7 @@
// A wrapper around the Trajectory flatbuffer to allow for controlling to a
// spline using a pre-generated trajectory.
-class FinishedTrajectory : public BaseTrajectory {
+class FinishedTrajectory : public BaseTrajectory {
public:
// Note: The lifetime of the supplied buffer is assumed to be greater than
// that of this object.
@@ -225,6 +225,11 @@
HybridKalman<2, 2, 2>>>(
config.make_hybrid_drivetrain_velocity_loop())) {}
+ FinishedTrajectory(const FinishedTrajectory &) = delete;
+ FinishedTrajectory &operator=(const FinishedTrajectory &) = delete;
+ FinishedTrajectory(FinishedTrajectory &&) = default;
+ FinishedTrajectory &operator=(FinishedTrajectory &&) = default;
+
virtual ~FinishedTrajectory() = default;
// Takes the 5-element state that is [x, y, theta, v_left, v_right] and
@@ -252,7 +257,7 @@
private:
const DistanceSplineBase &spline() const override { return spline_; }
const fb::Trajectory *buffer_;
- const FinishedDistanceSpline spline_;
+ FinishedDistanceSpline spline_;
};
// Class to handle plannign a trajectory and producing a flatbuffer containing
@@ -268,8 +273,7 @@
virtual ~Trajectory() = default;
- std::vector<Eigen::Matrix<double, 3, 1>> PlanXVA(
- std::chrono::nanoseconds dt);
+ std::vector<Eigen::Matrix<double, 3, 1>> PlanXVA(std::chrono::nanoseconds dt);
enum class VoltageLimit {
kConservative,
@@ -375,10 +379,7 @@
const DistanceSpline &spline() const override { return spline_; }
private:
-
- float plan_velocity(size_t index) const override {
- return plan_[index];
- }
+ float plan_velocity(size_t index) const override { return plan_[index]; }
size_t distance_plan_size() const override { return plan_.size(); }
fb::SegmentConstraint plan_constraint(size_t index) const override {
@@ -410,8 +411,7 @@
inline Eigen::Matrix<double, 5, 1> ContinuousDynamics(
const StateFeedbackHybridPlant<2, 2, 2> &velocity_drivetrain,
const Eigen::Matrix<double, 2, 2> &Tlr_to_la,
- const Eigen::Matrix<double, 5, 1> X,
- const Eigen::Matrix<double, 2, 1> U) {
+ const Eigen::Matrix<double, 5, 1> X, const Eigen::Matrix<double, 2, 1> U) {
const auto &velocity = X.block<2, 1>(3, 0);
const double theta = X(2);
Eigen::Matrix<double, 2, 1> la = Tlr_to_la * velocity;
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
index 178b63d..1cc6f57 100644
--- a/frc971/control_loops/python/graph.py
+++ b/frc971/control_loops/python/graph.py
@@ -49,12 +49,15 @@
if self.data is None:
return None
cursor_index = int(self.cursor / self.dt)
- if cursor_index > self.data.size:
+ if self.data[0].size < cursor_index:
return None
# use the time to index into the position data
- distance_at_cursor = self.data[0][cursor_index - 1]
- multispline_index = int(self.data[5][cursor_index - 1])
- return (multispline_index, distance_at_cursor)
+ try:
+ distance_at_cursor = self.data[0][cursor_index - 1]
+ multispline_index = int(self.data[5][cursor_index - 1])
+ return (multispline_index, distance_at_cursor)
+ except IndexError:
+ return None
def place_cursor(self, multispline_index, distance):
"""Places the cursor at a certain distance along the spline"""
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 0a944e1..2b55e94 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -240,8 +240,11 @@
multispline, result = Multispline.nearest_distance(
self.multisplines, mouse)
- if self.graph.cursor is not None and self.graph.data is not None:
- multispline_index, x = self.graph.find_cursor()
+ if self.graph.cursor is not None:
+ cursor = self.graph.find_cursor()
+ if cursor is None:
+ return
+ multispline_index, x = cursor
distance_spline = DistanceSpline(
self.multisplines[multispline_index].getLibsplines())
diff --git a/frc971/rockpi/build_rootfs.sh b/frc971/rockpi/build_rootfs.sh
index 74d4dc1..2e16419 100755
--- a/frc971/rockpi/build_rootfs.sh
+++ b/frc971/rockpi/build_rootfs.sh
@@ -16,6 +16,7 @@
bison
gcc-arm-none-eabi
gcc-aarch64-linux-gnu
+ u-boot-tools
device-tree-compiler
swig
debootstrap
@@ -164,14 +165,22 @@
-C "${PARTITION}/lib/modules/" ./lib/modules
# Now, configure it to start automatically.
-sudo mkdir -p ${PARTITION}/boot/extlinux/
-cat << __EOF__ | sudo tee "${PARTITION}/boot/extlinux/extlinux.conf"
+cat << __EOF__ | sudo tee "${PARTITION}/boot/sdcard_extlinux.conf"
label Linux ${KERNEL_VERSION}
kernel /vmlinuz-${KERNEL_VERSION}
append earlycon=uart8250,mmio32,0xff1a0000 earlyprintk console=ttyS2,1500000n8 root=/dev/mmcblk0p2 ro rootfstype=ext4 rootwait
fdtdir /dtbs/${KERNEL_VERSION}/
__EOF__
+cat << __EOF__ | sudo tee "${PARTITION}/boot/emmc_extlinux.conf"
+label Linux ${KERNEL_VERSION}
+ kernel /vmlinuz-${KERNEL_VERSION}
+ append earlycon=uart8250,mmio32,0xff1a0000 earlyprintk console=ttyS2,1500000n8 root=/dev/mmcblk1p2 ro rootfstype=ext4 rootwait
+ fdtdir /dtbs/${KERNEL_VERSION}/
+__EOF__
+mkimage -c none -A arm -T script -d contents/boot/boot.script contents/boot/boot.scr
+copyfile root.root 644 boot/boot.scr
+rm contents/boot/boot.scr
copyfile root.root 644 etc/apt/sources.list.d/bullseye-backports.list
copyfile root.root 644 etc/apt/sources.list.d/frc971.list
@@ -186,7 +195,7 @@
target "apt-get -y install -t bullseye-backports bpfcc-tools"
-target "apt-get install -y sudo openssh-server python3 bash-completion git v4l-utils cpufrequtils pmount rsync vim-nox chrony libopencv-calib3d4.5 libopencv-contrib4.5 libopencv-core4.5 libopencv-features2d4.5 libopencv-flann4.5 libopencv-highgui4.5 libopencv-imgcodecs4.5 libopencv-imgproc4.5 libopencv-ml4.5 libopencv-objdetect4.5 libopencv-photo4.5 libopencv-shape4.5 libopencv-stitching4.5 libopencv-superres4.5 libopencv-video4.5 libopencv-videoio4.5 libopencv-videostab4.5 libopencv-viz4.5 libnice10 pmount libnice-dev feh libgstreamer1.0-0 libgstreamer-plugins-base1.0-0 libgstreamer-plugins-bad1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-nice usbutils locales trace-cmd clinfo jq"
+target "apt-get install -y sudo openssh-server python3 bash-completion git v4l-utils cpufrequtils pmount rsync vim-nox chrony libopencv-calib3d4.5 libopencv-contrib4.5 libopencv-core4.5 libopencv-features2d4.5 libopencv-flann4.5 libopencv-highgui4.5 libopencv-imgcodecs4.5 libopencv-imgproc4.5 libopencv-ml4.5 libopencv-objdetect4.5 libopencv-photo4.5 libopencv-shape4.5 libopencv-stitching4.5 libopencv-superres4.5 libopencv-video4.5 libopencv-videoio4.5 libopencv-videostab4.5 libopencv-viz4.5 libnice10 pmount libnice-dev feh libgstreamer1.0-0 libgstreamer-plugins-base1.0-0 libgstreamer-plugins-bad1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-nice usbutils locales trace-cmd clinfo jq strace sysstat"
target "cd /tmp && wget https://software.frc971.org/Build-Dependencies/libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb && sudo dpkg -i libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb && rm libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb"
target "apt-get clean"
@@ -208,6 +217,7 @@
copyfile root.root 500 root/bin/deploy_kernel.sh
copyfile root.root 500 root/bin/chrt.sh
copyfile root.root 644 etc/systemd/system/grow-rootfs.service
+copyfile root.root 644 etc/systemd/system/mount-boot.service
copyfile root.root 644 etc/sysctl.d/sctp.conf
copyfile root.root 644 etc/systemd/logind.conf
copyfile root.root 555 etc/bash_completion.d/aos_dump_autocomplete
@@ -224,6 +234,7 @@
target "systemctl enable systemd-networkd"
target "systemctl enable grow-rootfs"
+target "systemctl enable mount-boot"
target "systemctl enable frc971"
target "systemctl enable frc971chrt"
target "/root/bin/change_hostname.sh pi-971-1"
diff --git a/frc971/rockpi/contents/boot/boot.script b/frc971/rockpi/contents/boot/boot.script
new file mode 100644
index 0000000..26732b6
--- /dev/null
+++ b/frc971/rockpi/contents/boot/boot.script
@@ -0,0 +1,6 @@
+if test ${devnum} -eq 1;
+then
+ sysboot ${devtype} ${devnum}:${distro_bootpart} any ${scriptaddr} sdcard_extlinux.conf
+else
+ sysboot ${devtype} ${devnum}:${distro_bootpart} any ${scriptaddr} emmc_extlinux.conf
+fi
diff --git a/frc971/rockpi/contents/etc/systemd/system/mount-boot.service b/frc971/rockpi/contents/etc/systemd/system/mount-boot.service
new file mode 100644
index 0000000..45e5783
--- /dev/null
+++ b/frc971/rockpi/contents/etc/systemd/system/mount-boot.service
@@ -0,0 +1,16 @@
+[Unit]
+Description=Mount /boot
+DefaultDependencies=no
+Before=local-fs.target
+Before=shutdown.target
+After=-.mount
+BindsTo=-.mount
+
+[Service]
+Type=oneshot
+ExecStart=/bin/bash -c "mount $(cat /proc/cmdline | sed 's/.*\(mmcblk.p\).*/\/dev\/\11/') /boot"
+TimeoutSec=0
+RemainAfterExit=yes
+
+[Install]
+WantedBy=multi-user.target
diff --git a/frc971/rockpi/contents/home/pi/.ssh/authorized_keys b/frc971/rockpi/contents/home/pi/.ssh/authorized_keys
index 9fd2597..58b82cc 100644
--- a/frc971/rockpi/contents/home/pi/.ssh/authorized_keys
+++ b/frc971/rockpi/contents/home/pi/.ssh/authorized_keys
@@ -1,4 +1,21 @@
ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDI002gCm4aRVrIcg2G4/qF4D1oNY74HFFAHjNUIgvrmqSEn+Oy+pxigpJFiZZaJMJpaw4kpd1IEpZxhooZm4DC4/bVV3wAFTw/OJI7D75WgrRrBRHd95TMBwYyNUhoDOcPAoZ69+IL9P0rhmNjgCv6Y+3PG+Rl6IqRPuf3dXX/PT3E/h8B18PRkEnas/3WTW8goov6x10kVAa5I+iQansiyAbPQF7E+Q5mpsnl26V2vpHo1UAk7y+TD7jqifEn13TmLeTkDXmaIOflQeOBMAdErftuqrClPa00VbejP18v02RI/jOIAQ250g0hN3zvKi2eNHUPdAzlMB4cSvZspRrB /home/austin/.ssh/id_rsa
+
ssh-rsa-cert-v01@openssh.com AAAAHHNzaC1yc2EtY2VydC12MDFAb3BlbnNzaC5jb20AAAAgzaqXNuB589EgR6/ljdYhp5Ca+B8eimCTmmC23oQvNyIAAAADAQABAAABAQDI002gCm4aRVrIcg2G4/qF4D1oNY74HFFAHjNUIgvrmqSEn+Oy+pxigpJFiZZaJMJpaw4kpd1IEpZxhooZm4DC4/bVV3wAFTw/OJI7D75WgrRrBRHd95TMBwYyNUhoDOcPAoZ69+IL9P0rhmNjgCv6Y+3PG+Rl6IqRPuf3dXX/PT3E/h8B18PRkEnas/3WTW8goov6x10kVAa5I+iQansiyAbPQF7E+Q5mpsnl26V2vpHo1UAk7y+TD7jqifEn13TmLeTkDXmaIOflQeOBMAdErftuqrClPa00VbejP18v02RI/jOIAQ250g0hN3zvKi2eNHUPdAzlMB4cSvZspRrBAAAAAAAAAAAAAAABAAAAHmF1c3Rpbi5zY2h1aEBibHVlcml2ZXJ0ZWNoLmNvbQAAAA8AAAADYnJ0AAAABHJvb3QAAAAAWGi3PAAAAABjhSpmAAAAAAAAAIIAAAAVcGVybWl0LVgxMS1mb3J3YXJkaW5nAAAAAAAAABdwZXJtaXQtYWdlbnQtZm9yd2FyZGluZwAAAAAAAAAWcGVybWl0LXBvcnQtZm9yd2FyZGluZwAAAAAAAAAKcGVybWl0LXB0eQAAAAAAAAAOcGVybWl0LXVzZXItcmMAAAAAAAAAAAAAAKwAAAATZWNkc2Etc2hhMi1uaXN0cDUyMQAAAAhuaXN0cDUyMQAAAIUEANvRmN8fXmKOO6xQPsllgbHxX+hvP4sU8/ayxw1K9C2MlGT3OKPgnjWEmvEPgpPR+/YQ6asQnP+jucdgCM8q7+c4ATwFnMO7yl2LCU1UkCKShzoumXflKC9rWNVT6MY4PTbpQXui5XE0gIZrjKrkcfCGjvRouUasM/C5Zro/aGQFkL6XAAAApwAAABNlY2RzYS1zaGEyLW5pc3RwNTIxAAAAjAAAAEIAswnueuP8iT7Qbzr1yBx6tLbNY9jewA6NEkLnFJtu11VzBFFaLWxeXwKPy3ajT0DCzt6EX6YKBHfYngnzdyjP9KkAAABCAWsxaA9D59ToYmbEKT//85dczH397v6As8WeQMAMzKfJYVSJBceHiwt6EbRKd6m+xUsd/Sr4Bj/Eu2VvwplqCpOq /home/austin/.ssh/id_rsa
+
ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDLyVTxr8s3kjz+PEjEymgwC8o64IZgBRJbtq9HZ16ufEHqxCD6WK21v8XsbAyTo3/vIfiae+SxhZTC9PMA1AQXuXCBTcvH1avHlHNPgnfxOfzNpU5LSZx/hqrx9tJ+ELV6m34XUbAhIhXJSyiPE2Mst8et6XUvXLgQ8hr0vwXZ3jitI0WzdoZE2svQhn/Cw+NnFiIyhVm4VTnw0bo5XVvvCawvZdTWsyXIvYx9P7rJ5Kvr1eJTZB+tDynzEFxJZeC+lnE6kV8NudC/7hLwwn1Uvqon17Z4P8ukxDsaD2Y4a2v0zqqN0FkEAKjhcqRWdyHM2JOeygRJa1sABNzt4gJB austin@ASchuh-T480s
+
ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQDDs88Uby+A7k69WSUXp6wmDrsIIFib5bsJ/0oHdjRWSZc4gHVGVF4cFCIJsjvuiQ3LlQ1vVb4ot7kXDNhPWEENRiuMVN3bovTr0fIjSi+YzhidIUZV44LhIkf2XorjpBjfdKE8YyZgYU0wway6myLijoEy6UnLaYerFjUh0k0p+R/axNtD48Glge82pvihosNt4J4592PGbfoTg7hgPizz4Yp39MtYN0OAqHDSrthU/ceA97prMo9tugozHthDasNAb1u/KiOr86dswLiGhwfM0aWAStIu+jie8fKzFtPFFvCyeEaGTYJ/nKiTq2qX2VNLk2zoqXoP6OPHTztejMtRyuRZxx3+mwhDT1lwUQx/ZsFqMTuIOjGQjpzQg3/Q2Y7rnSeQmgc5LglzaH5SRQ7i3nXJvDm1akdHRFFjarBw9Pb2p8DsDaTmJ6gpoEFqZZa1RM5ZCab8bL9z0pHBdpqhIXcPflDQE7Qi8win+LlWBwFyjhu5PvNnAKFEv6uQC1M= austin@aschuh-3950x
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQDONITvwkh8whp/ac9mhqIk+H4JV7gHIO6OYiOeJfd8mJGAKbox6JNPsnZ2Yewse9oc+MDKZ+dyLqBsfIVG0MUh96hJZHwIYW6wObcZt7Zj6c+JzbAQNavWltsu0IIA7wVrdz2qOXLuH3MhM5URP29IsmF9EUp2H1/iRUMlwz3+1GckYWmiJl+JKNct1DofJb4RdbY2iG34QgS6FvLEKZBxGYPboPD0DLylymdAZAq2TjCcngiq+5BWmy79BLoA2xwJjiz1WPbSILIygfX6e85FGf9ZskylfgHcltAP4xrg3ci7ygJZSw7rd37pV6PfxOz4vGmA7ROT+ocbkXtzkmGk6Qb8HPqT9uRl9Y2Wm4YQMIrREHvjXtGHePCS6R8rrK6og4U4l6Fn4ghicemoRcQXTeX8RJFN+jCpBmks+P6UbYW8k2424kVhNMHz2sHT+BU5Klz9HQvcqlCqupqiPT6HF/gzMZgMHAf2Dp17CpoDbhkhLmCE1TmFOKqNqHP31fs= milindupadhyay@mdu-brt
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDNUJEb8hoIboZ+FWUp9OX37xRs6s63pXfkF0Hirka+ovIziorNLxR6BixqD7xkVBwXgyMo+YRRy80eHLX2NoItoRLoMfT4uqhu/izdidLcUrpYicAxMIi1pCi1SS5BcKCHbyEqS5FrHzea+0dhIeemb1EP2NXxTDlFUINTni/Aw0z8bB3UI0tY0+K8zypuDKohL9tWOOln6e0VirhPhSCpfs89959jhmeI7Al1DuyZw1vfcaVgQ/7W59rHqP8RNcziMrwwt5hCMQQRxEu4Dd11jBiLklS+wLBI8exP/aEVXGwwc6D/zhg2zma4wQ7HuivZF+sMrP4T1RlLMK3ngbMX james@hp-debian
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQCaElJXdu6YMo7n32jAYXVPW9HKdp2vbE1BcKz+hqZDiIpMtIwmyu+KQD7XHOfkDrDLlsKRRnEvAKAhE+Q5apHdVwnR70h79khBjvRZqXBt+lip3GyGBuymE6+2bAL6zMnZ6oKBvKcPRdIUEbv1jvJV4SfCERGp0/yQxlf/h+yy2pGTEV2iGZerfYBDIyggDzrhjEeiFUL7In4kR2dQId/BPlzZx9AVIdF6CIY7MnfsPBZUkrk9XQrCJOxzN2gGPLdan3DMCp1o2K93dvojl4dQguphanwOdw8wm+wlMiucY/buHgqqbS0EFOz/taQDBFfQ2v7ckht5CFvCY56YpGFaVW7ztf7OYbU6t8LS3dsoN8+rAesBe9fJhJWR/0zFIAM7WW6j95Sppikqh0ZcaQK2/eMKFqn8R7K9zJ20BOI+QYGNnLvIVXlFzRt0/Qy/GczhejpJddwK9zbYxVKJyMNpVz3ZUlr+nRR2UtlcM+lWe7Kbu3HxTPBNk6vSCXPE+Pc= ravago@rjmacAir
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDSB1Nji8HEfnQX6E2FIfyvolrlihiBKukyLMA5isJhtvdaOXVOrSilpd91wB+ljLOfDy+4XWK4+8p84tvggdfvJdsLRtSBhExJNqe2ViyfUqh0O61lZg4Yvw6DGFEdS+DrqIkZh1v3WQ1Py1Mpt5V18dSu42DttHCigXFnYlpZfx9n4QT6GphkWYE3hLWHQH3uSkujzgkr86WreUi4idRDmq/r21H/MBx4q3uwWuftLS6oX79y7aukpieopeoWN8WUNscwUpUpmAn6rdFe/IHl0u8Zw4wmXoYSIQErFGsOK/rA3nKuK0uvVvUQkVKBZpjMOAAuaLugu9NYsOgqIZf/ filip@Filips-MacBook-Pro.local
+
+ssh-ed25519 AAAAC3NzaC1lZDI1NTE5AAAAILZrP0on5xPZHadvlMN2/+iZzEbIeGpS5MT5hXfb+kP0 uid@pop-os
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABgQC+t3HrUbZb9P/zWw9+qH9i8HBcCBRO6BS0rYZMz3UuXFSxhLendcqmAJQ+cu1lI0sb2HAyIhv4d5CQrjZ2en/ZyN93DThWNRWA+iH+ybkJhpUiRVSa/e1rz06o//kL+PVoZEHRUpyy7iJgEgcRxnVZd90baMkkm51bF0NSVn+iGCIao76VS+PBGwtQFghJyAvTz+w/hLYmFtXnY2DRfPvbw8H3cehvDhUBY9V+MUwucMU38Yfucr/BguPyw/sRFzRJtpIzP+JFQZzy/Bz4DI06vIWBps6NZqGo+ETXsrlEmNRKXgp3YWMZg/VcT6IIi9NijtDt6gHNDLfxX1Q41cYErBIaUpDKWPpGJ9z+/FxtxxBHwhIQPzSvdxqgsoGKyy7rFfWOPqWHzASsAcdx8V0EPIL4VmJQOd461WRfZxAhDdo2hQe37joRjcHW2i1nJcpVSepMf7EJH/AoYcifp+b+JUpMZx79HmKtE1cua2JcfZ6b20w7rQzLIP3M2WvrDak= deepachainani@Yashs-MBP
+
+ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQDG+8q7VDZv1bI2s/LDLZvjX4HInQobPF9QfBi8sNJdu98drWzSIlTIRMfOFuN8Vbq2E/wr6eKXOUwq/Brxu4sZ6+po6+sT6IqyE5cd+FffNJXZUc+QxXC6maD20i7k8gNJuo+BRa6VbSR25WarRoGT+pYl03jbhR304Wn8wIYIS5cAfaP2wzwNPOvCZzzwJrba2jeoeVWRUTuqLRAw4NNq5eZskdpv7w/3qw901z7RKzuTfYiAnvsRm8o6E0uRncyujzvXoXD2jyWdKC/boIv/l1Is/XwsSJahq1NIK3y95jZHoUXpDQsq49U0wOByBpKEyQYcI54nhfwYagVsYRY9 jim.o@bluerivert.com
diff --git a/frc971/rockpi/contents/run/systemd/resolve/stub-resolv.conf b/frc971/rockpi/contents/run/systemd/resolve/stub-resolv.conf
new file mode 100644
index 0000000..fa4bebb
--- /dev/null
+++ b/frc971/rockpi/contents/run/systemd/resolve/stub-resolv.conf
@@ -0,0 +1,3 @@
+domain lan
+search lan
+nameserver 127.0.0.53
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 642ca0f..3b011f2 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -99,11 +99,9 @@
"//aos/network:message_bridge_server_fbs",
"//aos/network:team_number",
"//frc971/control_loops:quaternion_utils",
+ "//frc971/vision:calibration_fbs",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
- "//y2020/vision/sift:sift_fbs",
- "//y2020/vision/sift:sift_training_fbs",
- "//y2020/vision/tools/python_code:sift_training_data",
"@com_github_foxglove_schemas//:schemas",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/strings:str_format",
@@ -303,3 +301,51 @@
"//aos/testing:tmpdir",
],
)
+
+cc_library(
+ name = "intrinsics_calibration_lib",
+ srcs = [
+ "intrinsics_calibration_lib.cc",
+ ],
+ hdrs = [
+ "intrinsics_calibration_lib.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:event_loop",
+ "//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:vision_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//frc971/wpilib:imu_fbs",
+ "//third_party:opencv",
+ "@com_google_absl//absl/strings:str_format",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_binary(
+ name = "intrinsics_calibration",
+ srcs = [
+ "intrinsics_calibration.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = [
+ "//y2020:__subpackages__",
+ "//y2022:__subpackages__",
+ "//y2023:__subpackages__",
+ ],
+ deps = [
+ ":intrinsics_calibration_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:vision_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//frc971/wpilib:imu_fbs",
+ "//third_party:opencv",
+ "@com_google_absl//absl/strings:str_format",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
diff --git a/frc971/vision/calibration.fbs b/frc971/vision/calibration.fbs
index f5e30a6..df8b7b0 100644
--- a/frc971/vision/calibration.fbs
+++ b/frc971/vision/calibration.fbs
@@ -44,6 +44,10 @@
// Timestamp for when the calibration was taken on the realtime clock.
calibration_timestamp:int64 (id: 6);
+
+ // ID for the physical camera hardware (typically will be a string of the form
+ // YY-NN, with a two-digit year and an index).
+ camera_id:string (id: 7);
}
// Calibration information for all the cameras we know about.
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index ea9af28..711ed7d 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -140,11 +140,13 @@
node, channel_overrides);
}
-Calibration::Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
- aos::EventLoop *image_event_loop,
- aos::EventLoop *imu_event_loop, std::string_view pi,
- TargetType target_type, std::string_view image_channel,
- CalibrationData *data)
+Calibration::Calibration(
+ aos::SimulatedEventLoopFactory *event_loop_factory,
+ aos::EventLoop *image_event_loop, aos::EventLoop *imu_event_loop,
+ std::string_view pi,
+ const calibration::CameraCalibration *intrinsics_calibration,
+ TargetType target_type, std::string_view image_channel,
+ CalibrationData *data)
: image_event_loop_(image_event_loop),
image_factory_(event_loop_factory->GetNodeEventLoopFactory(
image_event_loop_->node())),
@@ -152,7 +154,7 @@
imu_factory_(
event_loop_factory->GetNodeEventLoopFactory(imu_event_loop_->node())),
charuco_extractor_(
- image_event_loop_, pi, target_type, image_channel,
+ image_event_loop_, intrinsics_calibration, target_type, image_channel,
[this](cv::Mat rgb_image, monotonic_clock::time_point eof,
std::vector<cv::Vec4i> charuco_ids,
std::vector<std::vector<cv::Point2f>> charuco_corners,
diff --git a/frc971/vision/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index 5c435ad..c4dcae9 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -105,8 +105,10 @@
public:
Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
aos::EventLoop *image_event_loop, aos::EventLoop *imu_event_loop,
- std::string_view pi, TargetType target_type,
- std::string_view image_channel, CalibrationData *data);
+ std::string_view pi,
+ const calibration::CameraCalibration *intrinsics_calibration,
+ TargetType target_type, std::string_view image_channel,
+ CalibrationData *data);
// Processes a charuco detection that is returned from charuco_lib.
// For a valid detection(s), it stores camera observation
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index f12a6a5..116aba0 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -13,9 +13,6 @@
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/vision_generated.h"
#include "glog/logging.h"
-#include "y2020/vision/sift/sift_generated.h"
-#include "y2020/vision/sift/sift_training_generated.h"
-#include "y2020/vision/tools/python_code/sift_training_data.h"
DEFINE_string(board_template_path, "",
"If specified, write an image to the specified path for the "
@@ -37,59 +34,27 @@
using aos::monotonic_clock;
CameraCalibration::CameraCalibration(
- const absl::Span<const uint8_t> training_data_bfbs, std::string_view pi) {
- const aos::FlatbufferSpan<sift::TrainingData> training_data(
- training_data_bfbs);
- CHECK(training_data.Verify());
- camera_calibration_ = FindCameraCalibration(&training_data.message(), pi);
-}
-
-cv::Mat CameraCalibration::CameraIntrinsics() const {
- const cv::Mat result(3, 3, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration_->intrinsics()->data())));
- CHECK_EQ(result.total(), camera_calibration_->intrinsics()->size());
- return result;
-}
-
-Eigen::Matrix3d CameraCalibration::CameraIntrinsicsEigen() const {
- cv::Mat camera_intrinsics = CameraIntrinsics();
- Eigen::Matrix3d result;
- cv::cv2eigen(camera_intrinsics, result);
- return result;
-}
-
-cv::Mat CameraCalibration::CameraDistCoeffs() const {
- const cv::Mat result(5, 1, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration_->dist_coeffs()->data())));
- CHECK_EQ(result.total(), camera_calibration_->dist_coeffs()->size());
- return result;
-}
-
-const sift::CameraCalibration *CameraCalibration::FindCameraCalibration(
- const sift::TrainingData *const training_data, std::string_view pi) const {
- std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(pi);
- std::optional<uint16_t> team_number =
- aos::network::team_number_internal::ParsePiTeamNumber(pi);
- CHECK(pi_number);
- CHECK(team_number);
- const std::string node_name = absl::StrFormat("pi%d", pi_number.value());
- LOG(INFO) << "Looking for node name " << node_name << " team number "
- << team_number.value();
- for (const sift::CameraCalibration *candidate :
- *training_data->camera_calibrations()) {
- if (candidate->node_name()->string_view() != node_name) {
- continue;
- }
- if (candidate->team_number() != team_number.value()) {
- continue;
- }
- return candidate;
- }
- LOG(FATAL) << ": Failed to find camera calibration for " << node_name
- << " on " << team_number.value();
-}
+ const calibration::CameraCalibration *calibration)
+ : intrinsics_([calibration]() {
+ const cv::Mat result(3, 3, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ calibration->intrinsics()->data())));
+ CHECK_EQ(result.total(), calibration->intrinsics()->size());
+ return result;
+ }()),
+ intrinsics_eigen_([this]() {
+ cv::Mat camera_intrinsics = intrinsics_;
+ Eigen::Matrix3d result;
+ cv::cv2eigen(camera_intrinsics, result);
+ return result;
+ }()),
+ dist_coeffs_([calibration]() {
+ const cv::Mat result(5, 1, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ calibration->dist_coeffs()->data())));
+ CHECK_EQ(result.total(), calibration->dist_coeffs()->size());
+ return result;
+ }()) {}
ImageCallback::ImageCallback(
aos::EventLoop *event_loop, std::string_view channel,
@@ -257,8 +222,9 @@
const Eigen::Affine3d board_to_camera = translation * rotation;
- Eigen::Vector3d result = eigen_camera_matrix_ * camera_projection *
- board_to_camera * Eigen::Vector3d::Zero();
+ Eigen::Vector3d result = calibration_.CameraIntrinsicsEigen() *
+ camera_projection * board_to_camera *
+ Eigen::Vector3d::Zero();
// Found that drawAxis hangs if you try to draw with z values too
// small (trying to draw axes at inifinity)
@@ -269,11 +235,13 @@
} else {
result /= result.z();
if (target_type_ == TargetType::kCharuco) {
- cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvecs[i],
+ cv::aruco::drawAxis(rgb_image, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvecs[i],
tvecs[i], 0.1);
} else {
- cv::drawFrameAxes(rgb_image, camera_matrix_, dist_coeffs_, rvecs[i],
- tvecs[i], 0.1);
+ cv::drawFrameAxes(rgb_image, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvecs[i], tvecs[i],
+ 0.1);
}
}
std::stringstream ss;
@@ -307,7 +275,8 @@
}
CharucoExtractor::CharucoExtractor(
- aos::EventLoop *event_loop, std::string_view pi, TargetType target_type,
+ aos::EventLoop *event_loop,
+ const calibration::CameraCalibration *calibration, TargetType target_type,
std::string_view image_channel,
std::function<void(cv::Mat, monotonic_clock::time_point,
std::vector<cv::Vec4i>,
@@ -315,21 +284,14 @@
std::vector<Eigen::Vector3d>,
std::vector<Eigen::Vector3d>)> &&handle_charuco_fn)
: event_loop_(event_loop),
- calibration_(SiftTrainingData(), pi),
target_type_(target_type),
image_channel_(image_channel),
- camera_matrix_(calibration_.CameraIntrinsics()),
- eigen_camera_matrix_(calibration_.CameraIntrinsicsEigen()),
- dist_coeffs_(calibration_.CameraDistCoeffs()),
- pi_number_(aos::network::ParsePiNumber(pi)),
+ calibration_(CHECK_NOTNULL(calibration)),
handle_charuco_(std::move(handle_charuco_fn)) {
SetupTargetData();
- LOG(INFO) << "Camera matrix " << camera_matrix_;
- LOG(INFO) << "Distortion Coefficients " << dist_coeffs_;
-
- CHECK(pi_number_) << ": Invalid pi number " << pi
- << ", failed to parse pi number";
+ LOG(INFO) << "Camera matrix " << calibration_.CameraIntrinsics();
+ LOG(INFO) << "Distortion Coefficients " << calibration_.CameraDistCoeffs();
LOG(INFO) << "Connecting to channel " << image_channel_;
}
@@ -392,7 +354,7 @@
cv::aruco::interpolateCornersCharuco(
marker_corners, marker_ids, rgb_image, board_,
charuco_corners_with_calibration, charuco_ids_with_calibration,
- camera_matrix_, dist_coeffs_);
+ calibration_.CameraIntrinsics(), calibration_.CameraDistCoeffs());
if (charuco_ids.size() >= FLAGS_min_charucos) {
cv::aruco::drawDetectedCornersCharuco(
@@ -401,7 +363,8 @@
cv::Vec3d rvec, tvec;
valid = cv::aruco::estimatePoseCharucoBoard(
charuco_corners_with_calibration, charuco_ids_with_calibration,
- board_, camera_matrix_, dist_coeffs_, rvec, tvec);
+ board_, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvec, tvec);
// if charuco pose is valid, return pose, with ids and corners
if (valid) {
@@ -433,9 +396,9 @@
// estimate pose for arucos doesn't return valid, so marking true
valid = true;
std::vector<cv::Vec3d> rvecs, tvecs;
- cv::aruco::estimatePoseSingleMarkers(marker_corners, square_length_,
- camera_matrix_, dist_coeffs_, rvecs,
- tvecs);
+ cv::aruco::estimatePoseSingleMarkers(
+ marker_corners, square_length_, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvecs, tvecs);
DrawTargetPoses(rgb_image, rvecs, tvecs);
PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
@@ -459,9 +422,9 @@
// estimate pose for diamonds doesn't return valid, so marking true
valid = true;
std::vector<cv::Vec3d> rvecs, tvecs;
- cv::aruco::estimatePoseSingleMarkers(diamond_corners, square_length_,
- camera_matrix_, dist_coeffs_,
- rvecs, tvecs);
+ cv::aruco::estimatePoseSingleMarkers(
+ diamond_corners, square_length_, calibration_.CameraIntrinsics(),
+ calibration_.CameraDistCoeffs(), rvecs, tvecs);
DrawTargetPoses(rgb_image, rvecs, tvecs);
PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index 984cef6..b2ca7ee 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -11,8 +11,7 @@
#include "absl/types/span.h"
#include "aos/events/event_loop.h"
#include "aos/network/message_bridge_server_generated.h"
-#include "y2020/vision/sift/sift_generated.h"
-#include "y2020/vision/sift/sift_training_generated.h"
+#include "frc971/vision/calibration_generated.h"
#include "external/com_github_foxglove_schemas/ImageAnnotations_generated.h"
DECLARE_bool(visualize);
@@ -24,23 +23,19 @@
// training data.
class CameraCalibration {
public:
- CameraCalibration(const absl::Span<const uint8_t> training_data_bfbs,
- std::string_view pi);
+ CameraCalibration(const calibration::CameraCalibration *calibration);
// Intrinsics for the located camera.
- cv::Mat CameraIntrinsics() const;
- Eigen::Matrix3d CameraIntrinsicsEigen() const;
+ cv::Mat CameraIntrinsics() const { return intrinsics_; }
+ Eigen::Matrix3d CameraIntrinsicsEigen() const { return intrinsics_eigen_; }
// Distortion coefficients for the located camera.
- cv::Mat CameraDistCoeffs() const;
+ cv::Mat CameraDistCoeffs() const { return dist_coeffs_; }
private:
- // Finds the camera specific calibration flatbuffer.
- const sift::CameraCalibration *FindCameraCalibration(
- const sift::TrainingData *const training_data, std::string_view pi) const;
-
- // Pointer to this camera's calibration parameters.
- const sift::CameraCalibration *camera_calibration_;
+ const cv::Mat intrinsics_;
+ const Eigen::Matrix3d intrinsics_eigen_;
+ const cv::Mat dist_coeffs_;
};
// Helper class to call a function with a cv::Mat and age when an image shows up
@@ -108,7 +103,8 @@
// multiple targets in an image; for charuco boards, there should be just one
// element
CharucoExtractor(
- aos::EventLoop *event_loop, std::string_view pi, TargetType target_type,
+ aos::EventLoop *event_loop,
+ const calibration::CameraCalibration *calibration, TargetType target_type,
std::string_view image_channel,
std::function<void(cv::Mat, aos::monotonic_clock::time_point,
std::vector<cv::Vec4i>,
@@ -125,9 +121,11 @@
cv::Ptr<cv::aruco::CharucoBoard> board() const { return board_; }
// Returns the camera matrix for this camera.
- const cv::Mat camera_matrix() const { return camera_matrix_; }
+ const cv::Mat camera_matrix() const {
+ return calibration_.CameraIntrinsics();
+ }
// Returns the distortion coefficients for this camera.
- const cv::Mat dist_coeffs() const { return dist_coeffs_; }
+ const cv::Mat dist_coeffs() const { return calibration_.CameraDistCoeffs(); }
private:
// Creates the dictionary, board, and other parameters for the appropriate
@@ -146,7 +144,6 @@
std::vector<Eigen::Vector3d> *tvecs_eigen);
aos::EventLoop *event_loop_;
- CameraCalibration calibration_;
cv::Ptr<cv::aruco::Dictionary> dictionary_;
cv::Ptr<cv::aruco::CharucoBoard> board_;
@@ -161,15 +158,7 @@
// Length of a side of the checkerboard squares (around the marker)
double square_length_;
- // Intrinsic calibration matrix
- const cv::Mat camera_matrix_;
- // Intrinsic calibration matrix as Eigen::Matrix3d
- const Eigen::Matrix3d eigen_camera_matrix_;
- // Intrinsic distortion coefficients
- const cv::Mat dist_coeffs_;
-
- // Index number of the raspberry pi
- const std::optional<uint16_t> pi_number_;
+ CameraCalibration calibration_;
// Function to call.
std::function<void(
diff --git a/frc971/vision/intrinsics_calibration.cc b/frc971/vision/intrinsics_calibration.cc
new file mode 100644
index 0000000..224a37c
--- /dev/null
+++ b/frc971/vision/intrinsics_calibration.cc
@@ -0,0 +1,60 @@
+#include <cmath>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <regex>
+
+#include "absl/strings/str_format.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/network/team_number.h"
+#include "aos/time/time.h"
+#include "aos/util/file.h"
+#include "frc971/vision/intrinsics_calibration_lib.h"
+
+DEFINE_string(calibration_folder, ".", "Folder to place calibration files.");
+DEFINE_string(camera_id, "", "Camera ID in format YY-NN-- year and number.");
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+DEFINE_bool(display_undistorted, false,
+ "If true, display the undistorted image.");
+DEFINE_string(pi, "", "Pi name to calibrate.");
+DEFINE_string(base_intrinsics, "",
+ "Intrinsics to use for estimating board pose prior to solving "
+ "for the new intrinsics.");
+
+namespace frc971 {
+namespace vision {
+namespace {
+
+void Main() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ std::string hostname = FLAGS_pi;
+ if (hostname == "") {
+ hostname = aos::network::GetHostname();
+ LOG(INFO) << "Using pi name from hostname as " << hostname;
+ }
+ CHECK(!FLAGS_base_intrinsics.empty())
+ << "Need a base intrinsics json to use to auto-capture images when the "
+ "camera moves.";
+ std::unique_ptr<aos::ExitHandle> exit_handle = event_loop.MakeExitHandle();
+ IntrinsicsCalibration extractor(
+ &event_loop, hostname, FLAGS_camera_id, FLAGS_base_intrinsics,
+ FLAGS_display_undistorted, FLAGS_calibration_folder, exit_handle.get());
+
+ event_loop.Run();
+
+ extractor.MaybeCalibrate();
+}
+
+} // namespace
+} // namespace vision
+} // namespace frc971
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ frc971::vision::Main();
+}
diff --git a/frc971/vision/intrinsics_calibration_lib.cc b/frc971/vision/intrinsics_calibration_lib.cc
new file mode 100644
index 0000000..6cf53e8
--- /dev/null
+++ b/frc971/vision/intrinsics_calibration_lib.cc
@@ -0,0 +1,245 @@
+#include "frc971/vision/intrinsics_calibration_lib.h"
+
+namespace frc971 {
+namespace vision {
+
+IntrinsicsCalibration::IntrinsicsCalibration(
+ aos::EventLoop *event_loop, std::string_view pi, std::string_view camera_id,
+ std::string_view base_intrinsics_file, bool display_undistorted,
+ std::string_view calibration_folder, aos::ExitHandle *exit_handle)
+ : pi_(pi),
+ pi_number_(aos::network::ParsePiNumber(pi)),
+ camera_id_(camera_id),
+ prev_H_camera_board_(Eigen::Affine3d()),
+ prev_image_H_camera_board_(Eigen::Affine3d()),
+ base_intrinsics_(
+ aos::JsonFileToFlatbuffer<calibration::CameraCalibration>(
+ base_intrinsics_file)),
+ charuco_extractor_(
+ event_loop, &base_intrinsics_.message(), TargetType::kCharuco,
+ "/camera",
+ [this](cv::Mat rgb_image, const aos::monotonic_clock::time_point eof,
+ std::vector<cv::Vec4i> charuco_ids,
+ std::vector<std::vector<cv::Point2f>> charuco_corners,
+ bool valid, std::vector<Eigen::Vector3d> rvecs_eigen,
+ std::vector<Eigen::Vector3d> tvecs_eigen) {
+ HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
+ rvecs_eigen, tvecs_eigen);
+ }),
+ image_callback_(
+ event_loop,
+ absl::StrCat("/pi",
+ std::to_string(aos::network::ParsePiNumber(pi).value()),
+ "/camera"),
+ [this](cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof) {
+ charuco_extractor_.HandleImage(rgb_image, eof);
+ },
+ std::chrono::milliseconds(5)),
+ display_undistorted_(display_undistorted),
+ calibration_folder_(calibration_folder),
+ exit_handle_(exit_handle) {
+ CHECK(pi_number_) << ": Invalid pi number " << pi
+ << ", failed to parse pi number";
+ std::regex re{"^[0-9][0-9]-[0-9][0-9]"};
+ CHECK(std::regex_match(camera_id_, re))
+ << ": Invalid camera_id '" << camera_id_ << "', should be of form YY-NN";
+}
+
+void IntrinsicsCalibration::HandleCharuco(
+ cv::Mat rgb_image, const aos::monotonic_clock::time_point /*eof*/,
+ std::vector<cv::Vec4i> charuco_ids,
+ std::vector<std::vector<cv::Point2f>> charuco_corners, bool valid,
+ std::vector<Eigen::Vector3d> rvecs_eigen,
+ std::vector<Eigen::Vector3d> tvecs_eigen) {
+ // Reduce resolution displayed on remote viewer to prevent lag
+ cv::resize(rgb_image, rgb_image,
+ cv::Size(rgb_image.cols / 2, rgb_image.rows / 2));
+ cv::imshow("Display", rgb_image);
+
+ if (display_undistorted_) {
+ const cv::Size image_size(rgb_image.cols, rgb_image.rows);
+ cv::Mat undistorted_rgb_image(image_size, CV_8UC3);
+ cv::undistort(rgb_image, undistorted_rgb_image,
+ charuco_extractor_.camera_matrix(),
+ charuco_extractor_.dist_coeffs());
+
+ cv::imshow("Display undist", undistorted_rgb_image);
+ }
+
+ int keystroke = cv::waitKey(1);
+
+ // If we haven't got a valid pose estimate, don't use these points
+ if (!valid) {
+ return;
+ }
+ CHECK(tvecs_eigen.size() == 1)
+ << "Charuco board should only return one translational pose";
+ CHECK(rvecs_eigen.size() == 1)
+ << "Charuco board should only return one rotational pose";
+ // Calibration calculates rotation and translation delta from last image
+ // stored to automatically capture next image
+
+ Eigen::Affine3d H_board_camera =
+ Eigen::Translation3d(tvecs_eigen[0]) *
+ Eigen::AngleAxisd(rvecs_eigen[0].norm(),
+ rvecs_eigen[0] / rvecs_eigen[0].norm());
+ Eigen::Affine3d H_camera_board_ = H_board_camera.inverse();
+ Eigen::Affine3d H_delta = H_board_camera * prev_H_camera_board_;
+
+ Eigen::AngleAxisd delta_r = Eigen::AngleAxisd(H_delta.rotation());
+
+ Eigen::Vector3d delta_t = H_delta.translation();
+
+ double r_norm = std::abs(delta_r.angle());
+ double t_norm = delta_t.norm();
+
+ bool store_image = false;
+ double percent_motion =
+ std::max<double>(r_norm / kDeltaRThreshold, t_norm / kDeltaTThreshold);
+ LOG(INFO) << "Captured: " << all_charuco_ids_.size() << " points; Moved "
+ << percent_motion << "% of what's needed";
+ // Verify that camera has moved enough from last stored image
+ if (r_norm > kDeltaRThreshold || t_norm > kDeltaTThreshold) {
+ // frame_ refers to deltas between current and last captured image
+ Eigen::Affine3d frame_H_delta = H_board_camera * prev_image_H_camera_board_;
+
+ Eigen::AngleAxisd frame_delta_r =
+ Eigen::AngleAxisd(frame_H_delta.rotation());
+
+ Eigen::Vector3d frame_delta_t = frame_H_delta.translation();
+
+ double frame_r_norm = std::abs(frame_delta_r.angle());
+ double frame_t_norm = frame_delta_t.norm();
+
+ // Make sure camera has stopped moving before storing image
+ store_image =
+ frame_r_norm < kFrameDeltaRLimit && frame_t_norm < kFrameDeltaTLimit;
+ double percent_stop = std::max<double>(frame_r_norm / kFrameDeltaRLimit,
+ frame_t_norm / kFrameDeltaTLimit);
+ LOG(INFO) << "Captured: " << all_charuco_ids_.size()
+ << "points; Moved enough (" << percent_motion
+ << "%); Need to stop (last motion was " << percent_stop
+ << "% of limit; needs to be < 1 to capture)";
+ }
+ prev_image_H_camera_board_ = H_camera_board_;
+
+ if (store_image) {
+ if (valid) {
+ prev_H_camera_board_ = H_camera_board_;
+
+ // Unpack the Charuco ids from Vec4i
+ std::vector<int> charuco_ids_int;
+ for (cv::Vec4i charuco_id : charuco_ids) {
+ charuco_ids_int.emplace_back(charuco_id[0]);
+ }
+ all_charuco_ids_.emplace_back(std::move(charuco_ids_int));
+ all_charuco_corners_.emplace_back(std::move(charuco_corners[0]));
+
+ if (r_norm > kDeltaRThreshold) {
+ LOG(INFO) << "Triggered by rotation delta = " << r_norm << " > "
+ << kDeltaRThreshold;
+ }
+ if (t_norm > kDeltaTThreshold) {
+ LOG(INFO) << "Triggered by translation delta = " << t_norm << " > "
+ << kDeltaTThreshold;
+ }
+ }
+
+ } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
+ exit_handle_->Exit();
+ }
+}
+
+aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+IntrinsicsCalibration::BuildCalibration(
+ cv::Mat camera_matrix, cv::Mat dist_coeffs,
+ aos::realtime_clock::time_point realtime_now, int pi_number,
+ std::string_view camera_id, uint16_t team_number) {
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::Offset<flatbuffers::String> name_offset =
+ fbb.CreateString(absl::StrFormat("pi%d", pi_number));
+ flatbuffers::Offset<flatbuffers::String> camera_id_offset =
+ fbb.CreateString(camera_id);
+ flatbuffers::Offset<flatbuffers::Vector<float>> intrinsics_offset =
+ fbb.CreateVector<float>(9u, [&camera_matrix](size_t i) {
+ return static_cast<float>(
+ reinterpret_cast<double *>(camera_matrix.data)[i]);
+ });
+
+ flatbuffers::Offset<flatbuffers::Vector<float>>
+ distortion_coefficients_offset =
+ fbb.CreateVector<float>(5u, [&dist_coeffs](size_t i) {
+ return static_cast<float>(
+ reinterpret_cast<double *>(dist_coeffs.data)[i]);
+ });
+
+ calibration::CameraCalibration::Builder camera_calibration_builder(fbb);
+
+ camera_calibration_builder.add_node_name(name_offset);
+ camera_calibration_builder.add_team_number(team_number);
+ camera_calibration_builder.add_camera_id(camera_id_offset);
+ camera_calibration_builder.add_calibration_timestamp(
+ realtime_now.time_since_epoch().count());
+ camera_calibration_builder.add_intrinsics(intrinsics_offset);
+ camera_calibration_builder.add_dist_coeffs(distortion_coefficients_offset);
+ fbb.Finish(camera_calibration_builder.Finish());
+
+ return fbb.Release();
+}
+
+void IntrinsicsCalibration::MaybeCalibrate() {
+ // TODO: This number should depend on coarse vs. fine pattern
+ // Maybe just on total # of ids found, not just images
+ if (all_charuco_ids_.size() >= 50) {
+ LOG(INFO) << "Beginning calibration on " << all_charuco_ids_.size()
+ << " images";
+ cv::Mat camera_matrix, dist_coeffs;
+ std::vector<cv::Mat> rvecs, tvecs;
+ cv::Mat std_deviations_intrinsics, std_deviations_extrinsics,
+ per_view_errors;
+
+ // Set calibration flags (same as in calibrateCamera() function)
+ int calibration_flags = 0;
+ cv::Size img_size(640, 480);
+ const double reprojection_error = cv::aruco::calibrateCameraCharuco(
+ all_charuco_corners_, all_charuco_ids_, charuco_extractor_.board(),
+ img_size, camera_matrix, dist_coeffs, rvecs, tvecs,
+ std_deviations_intrinsics, std_deviations_extrinsics, per_view_errors,
+ calibration_flags);
+ CHECK_LE(reprojection_error, 1.0)
+ << ": Reproduction error is bad-- greater than 1 pixel.";
+ LOG(INFO) << "Reprojection Error is " << reprojection_error;
+
+ const aos::realtime_clock::time_point realtime_now =
+ aos::realtime_clock::now();
+ std::optional<uint16_t> team_number =
+ aos::network::team_number_internal::ParsePiTeamNumber(pi_);
+ CHECK(team_number) << ": Invalid pi hostname " << pi_
+ << ", failed to parse team number";
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ camera_calibration = BuildCalibration(camera_matrix, dist_coeffs,
+ realtime_now, pi_number_.value(),
+ camera_id_, team_number.value());
+ std::stringstream time_ss;
+ time_ss << realtime_now;
+
+ const std::string calibration_filename =
+ calibration_folder_ +
+ absl::StrFormat("/calibration_pi-%d-%d_cam-%s_%s.json",
+ team_number.value(), pi_number_.value(), camera_id_,
+ time_ss.str());
+
+ LOG(INFO) << calibration_filename << " -> "
+ << aos::FlatbufferToJson(camera_calibration,
+ {.multi_line = true});
+
+ aos::util::WriteStringToFileOrDie(
+ calibration_filename,
+ aos::FlatbufferToJson(camera_calibration, {.multi_line = true}));
+ } else {
+ LOG(INFO) << "Skipping calibration due to not enough images.";
+ }
+}
+} // namespace vision
+} // namespace frc971
diff --git a/frc971/vision/intrinsics_calibration_lib.h b/frc971/vision/intrinsics_calibration_lib.h
new file mode 100644
index 0000000..10062ba
--- /dev/null
+++ b/frc971/vision/intrinsics_calibration_lib.h
@@ -0,0 +1,78 @@
+#ifndef FRC971_VISION_CALIBRATION_LIB_H_
+#define FRC971_VISION_CALIBRATION_LIB_H_
+#include <cmath>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <regex>
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include "absl/strings/str_format.h"
+#include "aos/events/event_loop.h"
+#include "aos/network/team_number.h"
+#include "aos/time/time.h"
+#include "aos/util/file.h"
+#include "frc971/vision/charuco_lib.h"
+
+namespace frc971 {
+namespace vision {
+
+class IntrinsicsCalibration {
+ public:
+ IntrinsicsCalibration(aos::EventLoop *event_loop, std::string_view pi,
+ std::string_view camera_id,
+ std::string_view base_intrinsics_file,
+ bool display_undistorted,
+ std::string_view calibration_folder,
+ aos::ExitHandle *exit_handle);
+
+ void HandleCharuco(cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point /*eof*/,
+ std::vector<cv::Vec4i> charuco_ids,
+ std::vector<std::vector<cv::Point2f>> charuco_corners,
+ bool valid, std::vector<Eigen::Vector3d> rvecs_eigen,
+ std::vector<Eigen::Vector3d> tvecs_eigen);
+
+ void MaybeCalibrate();
+
+ static aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ BuildCalibration(cv::Mat camera_matrix, cv::Mat dist_coeffs,
+ aos::realtime_clock::time_point realtime_now, int pi_number,
+ std::string_view camera_id, uint16_t team_number);
+
+ private:
+ static constexpr double kDeltaRThreshold = M_PI / 6.0;
+ static constexpr double kDeltaTThreshold = 0.3;
+
+ static constexpr double kFrameDeltaRLimit = M_PI / 60;
+ static constexpr double kFrameDeltaTLimit = 0.01;
+
+ std::string pi_;
+ const std::optional<uint16_t> pi_number_;
+ const std::string camera_id_;
+
+ std::vector<std::vector<int>> all_charuco_ids_;
+ std::vector<std::vector<cv::Point2f>> all_charuco_corners_;
+
+ Eigen::Affine3d H_camera_board_;
+ Eigen::Affine3d prev_H_camera_board_;
+ Eigen::Affine3d prev_image_H_camera_board_;
+
+ // Camera intrinsics that we will use to bootstrap the intrinsics estimation
+ // here. We make use of the intrinsics in this calibration to allow us to
+ // estimate the relative pose of the charuco board and then identify how much
+ // the board is moving.
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
+ base_intrinsics_;
+ CharucoExtractor charuco_extractor_;
+ ImageCallback image_callback_;
+
+ const bool display_undistorted_;
+ const std::string calibration_folder_;
+ aos::ExitHandle *exit_handle_;
+};
+
+} // namespace vision
+} // namespace frc971
+#endif // FRC971_VISION_CALIBRATION_LIB_H_
diff --git a/tools/build_rules/jinja2_generator.py b/tools/build_rules/jinja2_generator.py
index e5f6fa3..3575e99 100644
--- a/tools/build_rules/jinja2_generator.py
+++ b/tools/build_rules/jinja2_generator.py
@@ -22,7 +22,8 @@
args = parser.parse_args(sys.argv[1:])
with open(args.template, 'r') as input_file:
- template = jinja2.Template(input_file.read())
+ template = jinja2.Environment(
+ loader=jinja2.FileSystemLoader(".")).from_string(input_file.read())
output = template.render(args.replacements)
with open(args.output, 'w') as config_file:
diff --git a/tools/build_rules/template.bzl b/tools/build_rules/template.bzl
index d4807e8..0b9167e 100644
--- a/tools/build_rules/template.bzl
+++ b/tools/build_rules/template.bzl
@@ -2,7 +2,7 @@
out = ctx.actions.declare_file(ctx.attr.name)
ctx.actions.run_shell(
- inputs = ctx.files.src,
+ inputs = ctx.files.src + ctx.files.includes,
tools = [ctx.executable._jinja2],
progress_message = "Generating " + out.short_path,
outputs = [out],
@@ -22,6 +22,10 @@
mandatory = True,
doc = """The parameters to supply to Jinja2.""",
),
+ "includes": attr.label_list(
+ allow_files = True,
+ doc = """Files which are included by the template.""",
+ ),
"_jinja2": attr.label(
default = "//tools/build_rules:jinja2_generator",
cfg = "host",
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index d05c245..f90cc92 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -82,7 +82,7 @@
":ekf",
":generated_graph",
":trajectory",
- "//third_party/matplotlib-cpp",
+ "//frc971/analysis:in_process_plotter",
"@com_github_gflags_gflags//:gflags",
"@org_tuxfamily_eigen//:eigen",
],
diff --git a/y2018/control_loops/superstructure/arm/trajectory_plot.cc b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
index eb3f7c1..10b39bc 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
@@ -1,10 +1,10 @@
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
-
+#include "aos/init.h"
+#include "frc971/analysis/in_process_plotter.h"
#include "gflags/gflags.h"
-#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/arm/ekf.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
+#include "y2018/control_loops/superstructure/arm/trajectory.h"
DEFINE_bool(forwards, true, "If true, run the forwards simulation.");
DEFINE_bool(plot, true, "If true, plot");
@@ -32,7 +32,7 @@
.finished();
trajectory.OptimizeTrajectory(alpha_unitizer, vmax);
- ::std::vector<double> distance_array = trajectory.DistanceArray();
+ const ::std::vector<double> distance_array = trajectory.DistanceArray();
::std::vector<double> theta0_array;
::std::vector<double> theta1_array;
@@ -96,7 +96,10 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t).array().max(-20).min(20);
+ Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ .array()
+ .max(-20)
+ .min(20);
Uff0_distance_array.push_back(U(0));
Uff1_distance_array.push_back(U(1));
@@ -115,17 +118,20 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t).array().max(-20).min(20);
+ Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ .array()
+ .max(-20)
+ .min(20);
Uff0_distance_array_curvature.push_back(U(0));
Uff1_distance_array_curvature.push_back(U(1));
}
for (const double distance : distance_array) {
- const double goal_velocity = trajectory.GetDVelocity(
- distance, trajectory.max_dvelocity());
- const double goal_acceleration = trajectory.GetDAcceleration(
- distance, trajectory.max_dvelocity());
+ const double goal_velocity =
+ trajectory.GetDVelocity(distance, trajectory.max_dvelocity());
+ const double goal_acceleration =
+ trajectory.GetDAcceleration(distance, trajectory.max_dvelocity());
const ::Eigen::Matrix<double, 2, 1> theta_t = trajectory.ThetaT(distance);
const ::Eigen::Matrix<double, 2, 1> omega_t =
trajectory.OmegaT(distance, goal_velocity);
@@ -134,7 +140,10 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t).array().max(-20).min(20);
+ Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ .array()
+ .max(-20)
+ .min(20);
Uff0_distance_array_backwards_only.push_back(U(0));
Uff1_distance_array_backwards_only.push_back(U(1));
@@ -244,115 +253,96 @@
}
if (FLAGS_plot) {
- matplotlibcpp::figure();
- matplotlibcpp::title("Trajectory");
- matplotlibcpp::plot(theta0_array, theta1_array,
- {{"label", "desired path"}});
- matplotlibcpp::plot(theta0_t_array, theta1_t_array,
- {{"label", "actual path"}});
- matplotlibcpp::legend();
+ frc971::analysis::Plotter plotter;
- matplotlibcpp::figure();
- matplotlibcpp::plot(distance_array, theta0_array, {{"label", "theta0"}});
- matplotlibcpp::plot(distance_array, theta1_array, {{"label", "theta1"}});
- matplotlibcpp::plot(distance_array, omega0_array, {{"label", "omega0"}});
- matplotlibcpp::plot(distance_array, omega1_array, {{"label", "omega1"}});
- matplotlibcpp::plot(distance_array, alpha0_array, {{"label", "alpha0"}});
- matplotlibcpp::plot(distance_array, alpha1_array, {{"label", "alpha1"}});
+ plotter.AddFigure();
+ plotter.Title("Trajectory");
+ plotter.AddLine(theta0_array, theta1_array, "desired path");
+ plotter.AddLine(theta0_t_array, theta1_t_array, "actual path");
+ plotter.Publish();
- matplotlibcpp::plot(integrated_distance, integrated_theta0_array,
- {{"label", "itheta0"}});
- matplotlibcpp::plot(integrated_distance, integrated_theta1_array,
- {{"label", "itheta1"}});
- matplotlibcpp::plot(integrated_distance, integrated_omega0_array,
- {{"label", "iomega0"}});
- matplotlibcpp::plot(integrated_distance, integrated_omega1_array,
- {{"label", "iomega1"}});
- matplotlibcpp::legend();
+ plotter.AddFigure();
+ plotter.Title("Input spline");
+ plotter.AddLine(distance_array, theta0_array, "theta0");
+ plotter.AddLine(distance_array, theta1_array, "theta1");
+ plotter.AddLine(distance_array, omega0_array, "omega0");
+ plotter.AddLine(distance_array, omega1_array, "omega1");
+ plotter.AddLine(distance_array, alpha0_array, "alpha0");
+ plotter.AddLine(distance_array, alpha1_array, "alpha1");
- matplotlibcpp::figure();
- matplotlibcpp::plot(distance_array, trajectory.max_dvelocity_unfiltered(),
- {{"label", "pass0"}});
- matplotlibcpp::plot(distance_array, trajectory.max_dvelocity(),
- {{"label", "passb"}});
- matplotlibcpp::plot(distance_array, trajectory.max_dvelocity_forward_pass(),
- {{"label", "passf"}});
- matplotlibcpp::legend();
+ plotter.AddLine(integrated_distance, integrated_theta0_array, "integrated theta0");
+ plotter.AddLine(integrated_distance, integrated_theta1_array, "integrated theta1");
+ plotter.AddLine(integrated_distance, integrated_omega0_array, "integrated omega0");
+ plotter.AddLine(integrated_distance, integrated_omega1_array, "integrated omega1");
+ plotter.Publish();
- matplotlibcpp::figure();
- matplotlibcpp::plot(t_array, alpha0_goal_t_array,
- {{"label", "alpha0_t_goal"}});
- matplotlibcpp::plot(t_array, alpha0_t_array, {{"label", "alpha0_t"}});
- matplotlibcpp::plot(t_array, alpha1_goal_t_array,
- {{"label", "alpha1_t_goal"}});
- matplotlibcpp::plot(t_array, alpha1_t_array, {{"label", "alpha1_t"}});
- matplotlibcpp::plot(t_array, distance_t_array, {{"label", "distance_t"}});
- matplotlibcpp::plot(t_array, velocity_t_array, {{"label", "velocity_t"}});
- matplotlibcpp::plot(t_array, acceleration_t_array,
- {{"label", "acceleration_t"}});
- matplotlibcpp::legend();
+ plotter.AddFigure();
+ plotter.Title("Solver passes");
+ plotter.AddLine(distance_array, trajectory.max_dvelocity_unfiltered(),
+ "pass0");
+ plotter.AddLine(distance_array, trajectory.max_dvelocity(), "passb");
+ plotter.AddLine(distance_array, trajectory.max_dvelocity_forward_pass(),
+ "passf");
+ plotter.Publish();
- matplotlibcpp::figure();
- matplotlibcpp::title("Angular Velocities");
- matplotlibcpp::plot(t_array, omega0_goal_t_array,
- {{"label", "omega0_t_goal"}});
- matplotlibcpp::plot(t_array, omega0_t_array, {{"label", "omega0_t"}});
- matplotlibcpp::plot(t_array, omega0_hat_t_array,
- {{"label", "omega0_hat_t"}});
- matplotlibcpp::plot(t_array, omega1_goal_t_array,
- {{"label", "omega1_t_goal"}});
- matplotlibcpp::plot(t_array, omega1_t_array, {{"label", "omega1_t"}});
- matplotlibcpp::plot(t_array, omega1_hat_t_array,
- {{"label", "omega1_hat_t"}});
- matplotlibcpp::legend();
+ plotter.AddFigure();
+ plotter.Title("Time Goals");
+ plotter.AddLine(t_array, alpha0_goal_t_array, "alpha0_t_goal");
+ plotter.AddLine(t_array, alpha0_t_array, "alpha0_t");
+ plotter.AddLine(t_array, alpha1_goal_t_array, "alpha1_t_goal");
+ plotter.AddLine(t_array, alpha1_t_array, "alpha1_t");
+ plotter.AddLine(t_array, distance_t_array, "distance_t");
+ plotter.AddLine(t_array, velocity_t_array, "velocity_t");
+ plotter.AddLine(t_array, acceleration_t_array, "acceleration_t");
+ plotter.Publish();
- matplotlibcpp::figure();
- matplotlibcpp::title("Voltages");
- matplotlibcpp::plot(t_array, u0_unsaturated_array, {{"label", "u0_full"}});
- matplotlibcpp::plot(t_array, u0_array, {{"label", "u0"}});
- matplotlibcpp::plot(t_array, uff0_array, {{"label", "uff0"}});
- matplotlibcpp::plot(t_array, u1_unsaturated_array, {{"label", "u1_full"}});
- matplotlibcpp::plot(t_array, u1_array, {{"label", "u1"}});
- matplotlibcpp::plot(t_array, uff1_array, {{"label", "uff1"}});
- matplotlibcpp::plot(t_array, torque0_hat_t_array,
- {{"label", "torque0_hat"}});
- matplotlibcpp::plot(t_array, torque1_hat_t_array,
- {{"label", "torque1_hat"}});
- matplotlibcpp::legend();
+ plotter.AddFigure();
+ plotter.Title("Angular Velocities");
+ plotter.AddLine(t_array, omega0_goal_t_array, "omega0_t_goal");
+ plotter.AddLine(t_array, omega0_t_array, "omega0_t");
+ plotter.AddLine(t_array, omega0_hat_t_array, "omega0_hat_t");
+ plotter.AddLine(t_array, omega1_goal_t_array, "omega1_t_goal");
+ plotter.AddLine(t_array, omega1_t_array, "omega1_t");
+ plotter.AddLine(t_array, omega1_hat_t_array, "omega1_hat_t");
+ plotter.Publish();
+
+ plotter.AddFigure();
+ plotter.Title("Voltages");
+ plotter.AddLine(t_array, u0_unsaturated_array, "u0_full");
+ plotter.AddLine(t_array, u0_array, "u0");
+ plotter.AddLine(t_array, uff0_array, "uff0");
+ plotter.AddLine(t_array, u1_unsaturated_array, "u1_full");
+ plotter.AddLine(t_array, u1_array, "u1");
+ plotter.AddLine(t_array, uff1_array, "uff1");
+ plotter.AddLine(t_array, torque0_hat_t_array, "torque0_hat");
+ plotter.AddLine(t_array, torque1_hat_t_array, "torque1_hat");
+ plotter.Publish();
if (FLAGS_plot_thetas) {
- matplotlibcpp::figure();
- matplotlibcpp::title("Angles");
- matplotlibcpp::plot(t_array, theta0_goal_t_array,
- {{"label", "theta0_t_goal"}});
- matplotlibcpp::plot(t_array, theta0_t_array, {{"label", "theta0_t"}});
- matplotlibcpp::plot(t_array, theta0_hat_t_array,
- {{"label", "theta0_hat_t"}});
- matplotlibcpp::plot(t_array, theta1_goal_t_array,
- {{"label", "theta1_t_goal"}});
- matplotlibcpp::plot(t_array, theta1_t_array, {{"label", "theta1_t"}});
- matplotlibcpp::plot(t_array, theta1_hat_t_array,
- {{"label", "theta1_hat_t"}});
- matplotlibcpp::legend();
+ plotter.AddFigure();
+ plotter.Title("Angles");
+ plotter.AddLine(t_array, theta0_goal_t_array, "theta0_t_goal");
+ plotter.AddLine(t_array, theta0_t_array, "theta0_t");
+ plotter.AddLine(t_array, theta0_hat_t_array, "theta0_hat_t");
+ plotter.AddLine(t_array, theta1_goal_t_array, "theta1_t_goal");
+ plotter.AddLine(t_array, theta1_t_array, "theta1_t");
+ plotter.AddLine(t_array, theta1_hat_t_array, "theta1_hat_t");
+ plotter.Publish();
}
- matplotlibcpp::figure();
- matplotlibcpp::title("ff for distance");
- matplotlibcpp::plot(distance_array, Uff0_distance_array, {{"label",
- "ff0"}});
- matplotlibcpp::plot(distance_array, Uff1_distance_array, {{"label",
- "ff1"}});
- matplotlibcpp::plot(distance_array, Uff0_distance_array_backwards_only,
- {{"label", "ff0_back"}});
- matplotlibcpp::plot(distance_array, Uff1_distance_array_backwards_only,
- {{"label", "ff1_back"}});
- matplotlibcpp::plot(distance_array, Uff0_distance_array_curvature,
- {{"label", "ff0_curve"}});
- matplotlibcpp::plot(distance_array, Uff1_distance_array_curvature,
- {{"label", "ff1_curve"}});
- matplotlibcpp::legend();
+ plotter.AddFigure();
+ plotter.Title("ff for distance");
+ plotter.AddLine(distance_array, Uff0_distance_array, "ff0");
+ plotter.AddLine(distance_array, Uff1_distance_array, "ff1");
+ plotter.AddLine(distance_array, Uff0_distance_array_backwards_only,
+ "ff0_back");
+ plotter.AddLine(distance_array, Uff1_distance_array_backwards_only,
+ "ff1_back");
+ plotter.AddLine(distance_array, Uff0_distance_array_curvature, "ff0_curve");
+ plotter.AddLine(distance_array, Uff1_distance_array_curvature, "ff1_curve");
- matplotlibcpp::show();
+ plotter.Publish();
+ plotter.Spin();
}
}
@@ -362,7 +352,7 @@
} // namespace y2018
int main(int argc, char **argv) {
- gflags::ParseCommandLineFlags(&argc, &argv, false);
+ ::aos::InitGoogle(&argc, &argv);
::y2018::control_loops::superstructure::arm::Main();
return 0;
}
diff --git a/y2020/BUILD b/y2020/BUILD
index 918ff79..2f6f963 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -35,7 +35,7 @@
robot_downloader(
name = "pi_download",
binaries = [
- "//y2020/vision:calibration",
+ "//frc971/vision:intrinsics_calibration",
"//y2020/vision:viewer",
],
data = [
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index 608ec5d..aa4e5bf 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -70,37 +70,6 @@
)
cc_binary(
- name = "calibration",
- srcs = [
- "calibration.cc",
- ],
- data = [
- "//y2020:aos_config",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = [
- "//y2020:__subpackages__",
- "//y2022:__subpackages__",
- "//y2023:__subpackages__",
- ],
- deps = [
- "//aos:init",
- "//aos/events:shm_event_loop",
- "//frc971/control_loops/drivetrain:improved_down_estimator",
- "//frc971/vision:charuco_lib",
- "//frc971/vision:vision_fbs",
- "//frc971/wpilib:imu_batch_fbs",
- "//frc971/wpilib:imu_fbs",
- "//third_party:opencv",
- "//y2020/vision/sift:sift_fbs",
- "//y2020/vision/sift:sift_training_fbs",
- "//y2020/vision/tools/python_code:sift_training_data",
- "@com_google_absl//absl/strings:str_format",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_binary(
name = "viewer_replay",
srcs = [
"viewer_replay.cc",
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
deleted file mode 100644
index d5ed54f..0000000
--- a/y2020/vision/calibration.cc
+++ /dev/null
@@ -1,303 +0,0 @@
-#include <cmath>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc.hpp>
-#include <regex>
-
-#include "Eigen/Dense"
-#include "Eigen/Geometry"
-#include "absl/strings/str_format.h"
-#include "aos/events/shm_event_loop.h"
-#include "aos/init.h"
-#include "aos/network/team_number.h"
-#include "aos/time/time.h"
-#include "aos/util/file.h"
-#include "frc971/vision/charuco_lib.h"
-
-DEFINE_string(calibration_folder, ".", "Folder to place calibration files.");
-DEFINE_string(camera_id, "", "Camera ID in format YY-NN-- year and number.");
-DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
-DEFINE_bool(display_undistorted, false,
- "If true, display the undistorted image.");
-DEFINE_string(pi, "", "Pi name to calibrate.");
-
-namespace frc971 {
-namespace vision {
-
-class Calibration {
- public:
- Calibration(aos::ShmEventLoop *event_loop, std::string_view pi,
- std::string_view camera_id)
- : event_loop_(event_loop),
- pi_(pi),
- pi_number_(aos::network::ParsePiNumber(pi)),
- camera_id_(camera_id),
- prev_H_camera_board_(Eigen::Affine3d()),
- prev_image_H_camera_board_(Eigen::Affine3d()),
- charuco_extractor_(
- event_loop, pi, TargetType::kCharuco, "/camera",
- [this](cv::Mat rgb_image,
- const aos::monotonic_clock::time_point eof,
- std::vector<cv::Vec4i> charuco_ids,
- std::vector<std::vector<cv::Point2f>> charuco_corners,
- bool valid, std::vector<Eigen::Vector3d> rvecs_eigen,
- std::vector<Eigen::Vector3d> tvecs_eigen) {
- HandleCharuco(rgb_image, eof, charuco_ids, charuco_corners, valid,
- rvecs_eigen, tvecs_eigen);
- }),
- image_callback_(
- event_loop,
- absl::StrCat(
- "/pi", std::to_string(aos::network::ParsePiNumber(pi).value()),
- "/camera"),
- [this](cv::Mat rgb_image,
- const aos::monotonic_clock::time_point eof) {
- charuco_extractor_.HandleImage(rgb_image, eof);
- },
- std::chrono::milliseconds(5)) {
- CHECK(pi_number_) << ": Invalid pi number " << pi
- << ", failed to parse pi number";
- std::regex re{"^[0-9][0-9]-[0-9][0-9]"};
- CHECK(std::regex_match(camera_id_, re))
- << ": Invalid camera_id '" << camera_id_
- << "', should be of form YY-NN";
- }
-
- void HandleCharuco(cv::Mat rgb_image,
- const aos::monotonic_clock::time_point /*eof*/,
- std::vector<cv::Vec4i> charuco_ids,
- std::vector<std::vector<cv::Point2f>> charuco_corners,
- bool valid, std::vector<Eigen::Vector3d> rvecs_eigen,
- std::vector<Eigen::Vector3d> tvecs_eigen) {
- // Reduce resolution displayed on remote viewer to prevent lag
- cv::resize(rgb_image, rgb_image,
- cv::Size(rgb_image.cols / 2, rgb_image.rows / 2));
- cv::imshow("Display", rgb_image);
-
- if (FLAGS_display_undistorted) {
- const cv::Size image_size(rgb_image.cols, rgb_image.rows);
- cv::Mat undistorted_rgb_image(image_size, CV_8UC3);
- cv::undistort(rgb_image, undistorted_rgb_image,
- charuco_extractor_.camera_matrix(),
- charuco_extractor_.dist_coeffs());
-
- cv::imshow("Display undist", undistorted_rgb_image);
- }
-
- int keystroke = cv::waitKey(1);
-
- // If we haven't got a valid pose estimate, don't use these points
- if (!valid) {
- return;
- }
- CHECK(tvecs_eigen.size() == 1)
- << "Charuco board should only return one translational pose";
- CHECK(rvecs_eigen.size() == 1)
- << "Charuco board should only return one rotational pose";
- // Calibration calculates rotation and translation delta from last image
- // stored to automatically capture next image
-
- Eigen::Affine3d H_board_camera =
- Eigen::Translation3d(tvecs_eigen[0]) *
- Eigen::AngleAxisd(rvecs_eigen[0].norm(),
- rvecs_eigen[0] / rvecs_eigen[0].norm());
- Eigen::Affine3d H_camera_board_ = H_board_camera.inverse();
- Eigen::Affine3d H_delta = H_board_camera * prev_H_camera_board_;
-
- Eigen::AngleAxisd delta_r = Eigen::AngleAxisd(H_delta.rotation());
-
- Eigen::Vector3d delta_t = H_delta.translation();
-
- double r_norm = std::abs(delta_r.angle());
- double t_norm = delta_t.norm();
-
- bool store_image = false;
- double percent_motion =
- std::max<double>(r_norm / kDeltaRThreshold, t_norm / kDeltaTThreshold);
- LOG(INFO) << "Captured: " << all_charuco_ids_.size() << " points; Moved "
- << percent_motion << "% of what's needed";
- // Verify that camera has moved enough from last stored image
- if (r_norm > kDeltaRThreshold || t_norm > kDeltaTThreshold) {
- // frame_ refers to deltas between current and last captured image
- Eigen::Affine3d frame_H_delta =
- H_board_camera * prev_image_H_camera_board_;
-
- Eigen::AngleAxisd frame_delta_r =
- Eigen::AngleAxisd(frame_H_delta.rotation());
-
- Eigen::Vector3d frame_delta_t = frame_H_delta.translation();
-
- double frame_r_norm = std::abs(frame_delta_r.angle());
- double frame_t_norm = frame_delta_t.norm();
-
- // Make sure camera has stopped moving before storing image
- store_image =
- frame_r_norm < kFrameDeltaRLimit && frame_t_norm < kFrameDeltaTLimit;
- double percent_stop = std::max<double>(frame_r_norm / kFrameDeltaRLimit,
- frame_t_norm / kFrameDeltaTLimit);
- LOG(INFO) << "Captured: " << all_charuco_ids_.size()
- << "points; Moved enough (" << percent_motion
- << "%); Need to stop (last motion was " << percent_stop
- << "% of limit; needs to be < 1 to capture)";
- }
- prev_image_H_camera_board_ = H_camera_board_;
-
- if (store_image) {
- if (valid) {
- prev_H_camera_board_ = H_camera_board_;
-
- // Unpack the Charuco ids from Vec4i
- std::vector<int> charuco_ids_int;
- for (cv::Vec4i charuco_id : charuco_ids) {
- charuco_ids_int.emplace_back(charuco_id[0]);
- }
- all_charuco_ids_.emplace_back(std::move(charuco_ids_int));
- all_charuco_corners_.emplace_back(std::move(charuco_corners[0]));
-
- if (r_norm > kDeltaRThreshold) {
- LOG(INFO) << "Triggered by rotation delta = " << r_norm << " > "
- << kDeltaRThreshold;
- }
- if (t_norm > kDeltaTThreshold) {
- LOG(INFO) << "Triggered by translation delta = " << t_norm << " > "
- << kDeltaTThreshold;
- }
- }
-
- } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
- event_loop_->Exit();
- }
- }
-
- void MaybeCalibrate() {
- // TODO: This number should depend on coarse vs. fine pattern
- // Maybe just on total # of ids found, not just images
- if (all_charuco_ids_.size() >= 50) {
- LOG(INFO) << "Beginning calibration on " << all_charuco_ids_.size()
- << " images";
- cv::Mat cameraMatrix, distCoeffs;
- std::vector<cv::Mat> rvecs, tvecs;
- cv::Mat stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors;
-
- // Set calibration flags (same as in calibrateCamera() function)
- int calibration_flags = 0;
- cv::Size img_size(640, 480);
- const double reprojection_error = cv::aruco::calibrateCameraCharuco(
- all_charuco_corners_, all_charuco_ids_, charuco_extractor_.board(),
- img_size, cameraMatrix, distCoeffs, rvecs, tvecs,
- stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors,
- calibration_flags);
- CHECK_LE(reprojection_error, 1.0)
- << ": Reproduction error is bad-- greater than 1 pixel.";
- LOG(INFO) << "Reprojection Error is " << reprojection_error;
-
- flatbuffers::FlatBufferBuilder fbb;
- flatbuffers::Offset<flatbuffers::String> name_offset =
- fbb.CreateString(absl::StrFormat("pi%d", pi_number_.value()));
- flatbuffers::Offset<flatbuffers::String> camera_id_offset =
- fbb.CreateString(camera_id_);
- flatbuffers::Offset<flatbuffers::Vector<float>> intrinsics_offset =
- fbb.CreateVector<float>(9u, [&cameraMatrix](size_t i) {
- return static_cast<float>(
- reinterpret_cast<double *>(cameraMatrix.data)[i]);
- });
-
- flatbuffers::Offset<flatbuffers::Vector<float>>
- distortion_coefficients_offset =
- fbb.CreateVector<float>(5u, [&distCoeffs](size_t i) {
- return static_cast<float>(
- reinterpret_cast<double *>(distCoeffs.data)[i]);
- });
-
- sift::CameraCalibration::Builder camera_calibration_builder(fbb);
- std::optional<uint16_t> team_number =
- aos::network::team_number_internal::ParsePiTeamNumber(pi_);
- CHECK(team_number) << ": Invalid pi hostname " << pi_
- << ", failed to parse team number";
-
- const aos::realtime_clock::time_point realtime_now =
- aos::realtime_clock::now();
- camera_calibration_builder.add_node_name(name_offset);
- camera_calibration_builder.add_team_number(team_number.value());
- camera_calibration_builder.add_camera_id(camera_id_offset);
- camera_calibration_builder.add_calibration_timestamp(
- realtime_now.time_since_epoch().count());
- camera_calibration_builder.add_intrinsics(intrinsics_offset);
- camera_calibration_builder.add_dist_coeffs(
- distortion_coefficients_offset);
- fbb.Finish(camera_calibration_builder.Finish());
-
- aos::FlatbufferDetachedBuffer<sift::CameraCalibration> camera_calibration(
- fbb.Release());
- std::stringstream time_ss;
- time_ss << realtime_now;
-
- const std::string calibration_filename =
- FLAGS_calibration_folder +
- absl::StrFormat("/calibration_pi-%d-%d_cam-%s_%s.json",
- team_number.value(), pi_number_.value(), camera_id_,
- time_ss.str());
-
- LOG(INFO) << calibration_filename << " -> "
- << aos::FlatbufferToJson(camera_calibration,
- {.multi_line = true});
-
- aos::util::WriteStringToFileOrDie(
- calibration_filename,
- aos::FlatbufferToJson(camera_calibration, {.multi_line = true}));
- } else {
- LOG(INFO) << "Skipping calibration due to not enough images.";
- }
- }
-
- private:
- static constexpr double kDeltaRThreshold = M_PI / 6.0;
- static constexpr double kDeltaTThreshold = 0.3;
-
- static constexpr double kFrameDeltaRLimit = M_PI / 60;
- static constexpr double kFrameDeltaTLimit = 0.01;
-
- aos::ShmEventLoop *event_loop_;
- std::string pi_;
- const std::optional<uint16_t> pi_number_;
- const std::string camera_id_;
-
- std::vector<std::vector<int>> all_charuco_ids_;
- std::vector<std::vector<cv::Point2f>> all_charuco_corners_;
-
- Eigen::Affine3d H_camera_board_;
- Eigen::Affine3d prev_H_camera_board_;
- Eigen::Affine3d prev_image_H_camera_board_;
-
- CharucoExtractor charuco_extractor_;
- ImageCallback image_callback_;
-};
-
-namespace {
-
-void Main() {
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
-
- aos::ShmEventLoop event_loop(&config.message());
-
- std::string hostname = FLAGS_pi;
- if (hostname == "") {
- hostname = aos::network::GetHostname();
- LOG(INFO) << "Using pi name from hostname as " << hostname;
- }
- Calibration extractor(&event_loop, hostname, FLAGS_camera_id);
-
- event_loop.Run();
-
- extractor.MaybeCalibrate();
-}
-
-} // namespace
-} // namespace vision
-} // namespace frc971
-
-int main(int argc, char **argv) {
- aos::InitGoogle(&argc, &argv);
- frc971::vision::Main();
-}
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index 3702355..d9e13b3 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -13,13 +13,12 @@
#include "frc971/vision/vision_generated.h"
#include "frc971/wpilib/imu_batch_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
-#include "y2020/vision/sift/sift_generated.h"
-#include "y2020/vision/sift/sift_training_generated.h"
-#include "y2020/vision/tools/python_code/sift_training_data.h"
DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
DEFINE_bool(plot, false, "Whether to plot the resulting data.");
DEFINE_bool(turret, false, "If true, the camera is on the turret");
+DEFINE_string(base_intrinsics, "",
+ "Intrinsics to use for extrinsics calibration.");
namespace frc971 {
namespace vision {
@@ -63,9 +62,13 @@
std::unique_ptr<aos::EventLoop> pi_event_loop =
factory.MakeEventLoop("calibration", pi_node);
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> intrinsics =
+ aos::JsonFileToFlatbuffer<calibration::CameraCalibration>(
+ FLAGS_base_intrinsics);
// Now, hook Calibration up to everything.
Calibration extractor(&factory, pi_event_loop.get(), imu_event_loop.get(),
- FLAGS_pi, TargetType::kCharuco, "/camera", &data);
+ FLAGS_pi, &intrinsics.message(), TargetType::kCharuco,
+ "/camera", &data);
if (FLAGS_turret) {
aos::NodeEventLoopFactory *roborio_factory =
diff --git a/y2022/BUILD b/y2022/BUILD
index 3d4dac3..5d32f15 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -39,7 +39,7 @@
robot_downloader(
name = "pi_download",
binaries = [
- "//y2020/vision:calibration",
+ "//frc971/vision:intrinsics_calibration",
"//y2022/vision:viewer",
"//y2022/localizer:imu_main",
"//y2022/localizer:localizer_main",
diff --git a/y2022/vision/calibrate_extrinsics.cc b/y2022/vision/calibrate_extrinsics.cc
index 04b24ea..6793bc4 100644
--- a/y2022/vision/calibrate_extrinsics.cc
+++ b/y2022/vision/calibrate_extrinsics.cc
@@ -11,9 +11,6 @@
#include "frc971/vision/extrinsics_calibration.h"
#include "frc971/vision/vision_generated.h"
#include "frc971/wpilib/imu_batch_generated.h"
-#include "y2020/vision/sift/sift_generated.h"
-#include "y2020/vision/sift/sift_training_generated.h"
-#include "y2020/vision/tools/python_code/sift_training_data.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
@@ -24,6 +21,8 @@
DEFINE_string(image_channel, "/camera", "Channel to listen for images on");
DEFINE_string(output_logs, "/tmp/calibration/",
"Output folder for visualization logs.");
+DEFINE_string(base_intrinsics, "",
+ "Intrinsics to use for extrinsics calibration.");
namespace frc971 {
namespace vision {
@@ -97,9 +96,13 @@
<< ", expected: aruco|charuco|charuco_diamond";
}
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> intrinsics =
+ aos::JsonFileToFlatbuffer<calibration::CameraCalibration>(
+ FLAGS_base_intrinsics);
// Now, hook Calibration up to everything.
Calibration extractor(&factory, pi_event_loop.get(), imu_event_loop.get(),
- FLAGS_pi, target_type, FLAGS_image_channel, &data);
+ FLAGS_pi, &intrinsics.message(), target_type,
+ FLAGS_image_channel, &data);
if (FLAGS_turret) {
aos::NodeEventLoopFactory *roborio_factory =
diff --git a/y2023/BUILD b/y2023/BUILD
index 3a5f1bd..6ba1392 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -5,17 +5,20 @@
robot_downloader(
name = "pi_download",
binaries = [
- "//y2020/vision:calibration",
+ "//frc971/vision:intrinsics_calibration",
"//y2023/vision:viewer",
"//y2023/vision:aprilrobotics",
"//y2022/localizer:localizer_main",
+ "//y2023/constants:constants_sender",
"//aos/network:web_proxy_main",
"//aos/events/logging:log_cat",
+ "//y2023/rockpi:imu_main",
],
data = [
":aos_config",
":message_bridge_client.sh",
- "//y2022/www:www_files",
+ "//y2023/constants:constants.json",
+ "//y2023/www:www_files",
],
dirs = [
"//y2023/www:www_files",
@@ -65,10 +68,12 @@
"//aos/network:message_bridge_server_fbs",
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
+ "//y2023/constants:constants_fbs",
"//y2022/localizer:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
"//frc971/vision:vision_fbs",
+ "//y2023/vision:april_debug_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -92,6 +97,7 @@
flatbuffers = [
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
+ "//y2023/constants:constants_fbs",
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//y2022/localizer:localizer_status_fbs",
@@ -118,6 +124,7 @@
"//frc971/vision:calibration_fbs",
"//frc971/vision:vision_fbs",
"//frc971/vision:target_map_fbs",
+ "//y2023/constants:constants_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -135,6 +142,7 @@
"//aos/network:remote_message_fbs",
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
+ "//y2023/constants:constants_fbs",
"//aos/network:timestamp_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
"//y2023/control_loops/superstructure:superstructure_goal_fbs",
diff --git a/y2023/constants/7971.json b/y2023/constants/7971.json
new file mode 100644
index 0000000..ded4869
--- /dev/null
+++ b/y2023/constants/7971.json
@@ -0,0 +1,16 @@
+{
+ "cameras": [
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json' %}
+ },
+ {
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json' %}
+ }
+ ]
+}
diff --git a/y2023/constants/BUILD b/y2023/constants/BUILD
new file mode 100644
index 0000000..360ecc2
--- /dev/null
+++ b/y2023/constants/BUILD
@@ -0,0 +1,54 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//tools/build_rules:template.bzl", "jinja2_template")
+
+cc_library(
+ name = "simulated_constants_sender",
+ testonly = True,
+ srcs = ["simulated_constants_sender.h"],
+ hdrs = ["simulated_constants_sender.cc"],
+ deps = [
+ ":constants_fbs",
+ ":constants_list_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:path",
+ "//frc971/constants:constants_sender_lib",
+ ],
+)
+
+jinja2_template(
+ name = "constants.json",
+ src = "constants.jinja2.json",
+ includes = ["//y2023/vision/calib_files"] + ["7971.json"],
+ parameters = {},
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_cc_library(
+ name = "constants_fbs",
+ srcs = ["constants.fbs"],
+ gen_reflections = True,
+ visibility = ["//visibility:public"],
+ deps = ["//frc971/vision:calibration_fbs"],
+)
+
+flatbuffer_cc_library(
+ name = "constants_list_fbs",
+ srcs = ["constants_list.fbs"],
+ gen_reflections = True,
+ visibility = ["//visibility:public"],
+ deps = [":constants_fbs"],
+)
+
+cc_binary(
+ name = "constants_sender",
+ srcs = ["constants_sender.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":constants_fbs",
+ ":constants_list_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/testing:path",
+ "//frc971/constants:constants_sender_lib",
+ ],
+)
diff --git a/y2023/constants/constants.fbs b/y2023/constants/constants.fbs
new file mode 100644
index 0000000..e62e253
--- /dev/null
+++ b/y2023/constants/constants.fbs
@@ -0,0 +1,13 @@
+include "frc971/vision/calibration.fbs";
+
+namespace y2023;
+
+table CameraConfiguration {
+ calibration:frc971.vision.calibration.CameraCalibration (id: 0);
+}
+
+table Constants {
+ cameras:[CameraConfiguration] (id: 0);
+}
+
+root_type Constants;
diff --git a/y2023/constants/constants.jinja2.json b/y2023/constants/constants.jinja2.json
new file mode 100644
index 0000000..6a8cde9
--- /dev/null
+++ b/y2023/constants/constants.jinja2.json
@@ -0,0 +1,16 @@
+{
+ "constants": [
+ {
+ "team": 7971,
+ "data": {% include 'y2023/constants/7971.json' %}
+ },
+ {
+ "team": 971,
+ "data": {}
+ },
+ {
+ "team": 9971,
+ "data": {}
+ }
+ ]
+}
diff --git a/y2023/constants/constants_list.fbs b/y2023/constants/constants_list.fbs
new file mode 100644
index 0000000..aec10d8
--- /dev/null
+++ b/y2023/constants/constants_list.fbs
@@ -0,0 +1,14 @@
+include "y2023/constants/constants.fbs";
+
+namespace y2023;
+
+table TeamAndConstants {
+ team:long (id: 0);
+ data:Constants (id: 1);
+}
+
+table ConstantsList {
+ constants:[TeamAndConstants] (id: 0);
+}
+
+root_type ConstantsList;
diff --git a/y2023/constants/constants_sender.cc b/y2023/constants/constants_sender.cc
new file mode 100644
index 0000000..77fb24d
--- /dev/null
+++ b/y2023/constants/constants_sender.cc
@@ -0,0 +1,23 @@
+#include "aos/configuration.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+#include "y2023/constants/constants_generated.h"
+#include "y2023/constants/constants_list_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the AOS config.");
+DEFINE_string(constants_path, "constants.json", "Path to the constant file");
+
+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());
+ frc971::constants::ConstantSender<y2023::Constants, y2023::ConstantsList>
+ constants_sender(&event_loop, FLAGS_constants_path);
+ // Don't need to call Run().
+ return 0;
+}
diff --git a/y2023/constants/simulated_constants_sender.cc b/y2023/constants/simulated_constants_sender.cc
new file mode 100644
index 0000000..3086e99
--- /dev/null
+++ b/y2023/constants/simulated_constants_sender.cc
@@ -0,0 +1,18 @@
+#include "y2023/constants/constants_generated.h"
+#include "y2023/constants/constants_list_generated.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+
+namespace y2023 {
+void SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path) {
+ for (const aos::Node *node : factory->nodes()) {
+ std::unique_ptr<aos::EventLoop> event_loop =
+ factory->MakeEventLoop("constants_sender", node);
+ frc971::constants::ConstantSender<Constants, ConstantsList> sender(
+ event_loop.get(), constants_path, team, "/constants");
+ }
+}
+} // namespace y2023
+
+#endif // Y2023_CONFIGURATION_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2023/constants/simulated_constants_sender.h b/y2023/constants/simulated_constants_sender.h
new file mode 100644
index 0000000..10ea793
--- /dev/null
+++ b/y2023/constants/simulated_constants_sender.h
@@ -0,0 +1,13 @@
+#ifndef Y2023_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+#define Y2023_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+
+namespace y2023 {
+void SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path = testing::ArtifactPath(
+ "y2023/constants/constants.json"));
+} // namespace y2023
+
+#endif // Y2023_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index 39cd814..448103b 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -361,12 +361,13 @@
cr.stroke()
def cur_pt_in_theta(self):
- if self.theta_version: return self.last_pos
+ if self.theta_version: return numpy.asarray(self.last_pos)
return to_theta(self.last_pos, self.circular_index_select)
# Current segment based on which mode the drawing system is in.
def current_seg(self):
- if self.prev_segment_pt and self.now_segment_pt:
+ if self.prev_segment_pt is not None and (self.prev_segment_pt.any() and
+ self.now_segment_pt.any()):
if self.theta_version:
return AngleSegment(self.prev_segment_pt, self.now_segment_pt)
else:
diff --git a/y2023/foxglove-layouts/2022RobotCameras.json b/y2023/foxglove-layouts/2022RobotCameras.json
new file mode 100644
index 0000000..88540f0
--- /dev/null
+++ b/y2023/foxglove-layouts/2022RobotCameras.json
@@ -0,0 +1,114 @@
+{
+ "configById": {
+ "ImageViewPanel!3j0qvlm": {
+ "cameraTopic": "/studio_script/YUYVConvertedImagePi1",
+ "enabledMarkerTopics": [],
+ "mode": "fit",
+ "pan": {
+ "x": 0,
+ "y": 0
+ },
+ "rotation": 0,
+ "synchronize": false,
+ "transformMarkers": false,
+ "zoom": 1
+ },
+ "ImageViewPanel!1uof9zo": {
+ "cameraTopic": "/studio_script/YUYVConvertedImagePi3",
+ "enabledMarkerTopics": [],
+ "mode": "fit",
+ "pan": {
+ "x": 0,
+ "y": 0
+ },
+ "rotation": 0,
+ "synchronize": false,
+ "transformMarkers": false,
+ "zoom": 1
+ },
+ "ImageViewPanel!1ya4lw8": {
+ "cameraTopic": "/studio_script/YUYVConvertedImagePi2",
+ "enabledMarkerTopics": [],
+ "mode": "fit",
+ "pan": {
+ "x": 0,
+ "y": 0
+ },
+ "rotation": 0,
+ "synchronize": false,
+ "transformMarkers": false,
+ "zoom": 1
+ },
+ "ImageViewPanel!1tvn3cp": {
+ "cameraTopic": "/studio_script/YUYVConvertedImagePi4",
+ "enabledMarkerTopics": [],
+ "mode": "fit",
+ "pan": {
+ "x": 0,
+ "y": 0
+ },
+ "rotation": 0,
+ "synchronize": false,
+ "transformMarkers": false,
+ "zoom": 1
+ },
+ "ImageViewPanel!jmmyy1": {
+ "cameraTopic": "/studio_script/YUYVConvertedImageDriverCamera",
+ "enabledMarkerTopics": [],
+ "mode": "fit",
+ "pan": {
+ "x": 1,
+ "y": -1
+ },
+ "rotation": 0,
+ "synchronize": false,
+ "transformMarkers": false,
+ "zoom": 1
+ }
+ },
+ "globalVariables": {},
+ "userNodes": {
+ "1eb832cf-fe38-4d19-9f6b-be4144f8c594": {
+ "sourceCode": "// The ./types module provides helper types for your Input events and messages.\nimport { Input, Message, Time } from \"./types\";\n\n// Your script can output well-known message types, any of your custom message types, or\n// complete custom message types.\n//\n// Use `Message` to access your data source types or well-known types:\n// type Twist = Message<\"geometry_msgs/Twist\">;\n\n// These are the topics your script \"subscribes\" to. Studio will invoke your script function\n// when any message is received on one of these topics.\nexport const inputs = [\"/camera/decimated frc971.vision.CameraImage\"];\n\n// Any output your script produces is \"published\" to this topic. Published messages are only visible within Studio, not to your original data source.\nexport const output = \"/studio_script/YUYVConvertedImageDriverCamera\";\n\n// This function is called with messages from your input topics.\n// The first argument is an event with the topic, receive time, and message.\n// Use the `Input<...>` helper to get the correct event type for your input topic messages.\nexport default function script(\n event: Input<\"/camera/decimated frc971.vision.CameraImage\">\n): Message<\"foxglove_msgs/RawImage\"> {\n let timestampmt: Time;\n timestampmt = {\n sec: Number(event.message.monotonic_timestamp_ns) / 1000000000,\n nsec: Number(event.message.monotonic_timestamp_ns),\n };\n const data = new Uint8Array(event.message.data.length);\n for (let ii = 0; ii < data.length; ii += 4) {\n data[ii] = event.message.data[ii + 1];\n data[ii + 1] = event.message.data[ii];\n data[ii + 2] = event.message.data[ii + 3];\n data[ii + 3] = event.message.data[ii + 2];\n }\n return {\n timestamp: timestampmt,\n frame_id: \"DRIVER_CAMERA\",\n width: event.message.cols,\n height: event.message.rows,\n encoding: \"yuv422\",\n step: 1280,\n data: data,\n };\n}\n",
+ "name": "1eb832cf"
+ },
+ "20d0b3c5-95ed-4825-bc51-cdc20182b2c1": {
+ "sourceCode": "// The ./types module provides helper types for your Input events and messages.\nimport { Input, Message, Time } from \"./types\";\n\n// Your script can output well-known message types, any of your custom message types, or\n// complete custom message types.\n//\n// Use `Message` to access your data source types or well-known types:\n// type Twist = Message<\"geometry_msgs/Twist\">;\n\n// These are the topics your script \"subscribes\" to. Studio will invoke your script function\n// when any message is received on one of these topics.\nexport const inputs = [\"/pi1/camera/decimated frc971.vision.CameraImage\"];\n\n// Any output your script produces is \"published\" to this topic. Published messages are only visible within Studio, not to your original data source.\nexport const output = \"/studio_script/YUYVConvertedImagePi1\";\n\n// This function is called with messages from your input topics.\n// The first argument is an event with the topic, receive time, and message.\n// Use the `Input<...>` helper to get the correct event type for your input topic messages.\nexport default function script(\n event: Input<\"/pi1/camera/decimated frc971.vision.CameraImage\">\n): Message<\"foxglove_msgs/RawImage\"> {\n let timestampmt: Time;\n timestampmt = {\n sec: Number(event.message.monotonic_timestamp_ns) / 1000000000,\n nsec: Number(event.message.monotonic_timestamp_ns),\n };\n const data = new Uint8Array(event.message.data.length);\n for (let ii = 0; ii < data.length; ii += 4) {\n data[ii] = event.message.data[ii + 1];\n data[ii + 1] = event.message.data[ii];\n data[ii + 2] = event.message.data[ii + 3];\n data[ii + 3] = event.message.data[ii + 2];\n }\n return {\n timestamp: timestampmt,\n frame_id: \"PI1\",\n width: event.message.cols,\n height: event.message.rows,\n encoding: \"yuv422\",\n step: 1280,\n data: data,\n };\n}\n",
+ "name": "20d0b3c5"
+ },
+ "8837da0e-fd3d-4f76-be0c-d93e842db0b4": {
+ "sourceCode": "// The ./types module provides helper types for your Input events and messages.\nimport { Input, Message, Time } from \"./types\";\n\n// Your script can output well-known message types, any of your custom message types, or\n// complete custom message types.\n//\n// Use `Message` to access your data source types or well-known types:\n// type Twist = Message<\"geometry_msgs/Twist\">;\n\n// These are the topics your script \"subscribes\" to. Studio will invoke your script function\n// when any message is received on one of these topics.\nexport const inputs = [\"/pi2/camera/decimated frc971.vision.CameraImage\"];\n\n// Any output your script produces is \"published\" to this topic. Published messages are only visible within Studio, not to your original data source.\nexport const output = \"/studio_script/YUYVConvertedImagePi2\";\n\n// This function is called with messages from your input topics.\n// The first argument is an event with the topic, receive time, and message.\n// Use the `Input<...>` helper to get the correct event type for your input topic messages.\nexport default function script(\n event: Input<\"/pi2/camera/decimated frc971.vision.CameraImage\">\n): Message<\"foxglove_msgs/RawImage\"> {\n let timestampmt: Time;\n timestampmt = {\n sec: Number(event.message.monotonic_timestamp_ns) / 1000000000,\n nsec: Number(event.message.monotonic_timestamp_ns),\n };\n const data = new Uint8Array(event.message.data.length);\n for (let ii = 0; ii < data.length; ii += 4) {\n data[ii] = event.message.data[ii + 1];\n data[ii + 1] = event.message.data[ii];\n data[ii + 2] = event.message.data[ii + 3];\n data[ii + 3] = event.message.data[ii + 2];\n }\n return {\n timestamp: timestampmt,\n frame_id: \"PI2\",\n width: event.message.cols,\n height: event.message.rows,\n encoding: \"yuv422\",\n step: 1280,\n data: data,\n };\n}\n",
+ "name": "8837da0e"
+ },
+ "86094f4e-ccf3-464b-848d-4fa3b4a62fe0": {
+ "sourceCode": "// The ./types module provides helper types for your Input events and messages.\nimport { Input, Message, Time } from \"./types\";\n\n// Your script can output well-known message types, any of your custom message types, or\n// complete custom message types.\n//\n// Use `Message` to access your data source types or well-known types:\n// type Twist = Message<\"geometry_msgs/Twist\">;\n\n// These are the topics your script \"subscribes\" to. Studio will invoke your script function\n// when any message is received on one of these topics.\nexport const inputs = [\"/pi3/camera/decimated frc971.vision.CameraImage\"];\n\n// Any output your script produces is \"published\" to this topic. Published messages are only visible within Studio, not to your original data source.\nexport const output = \"/studio_script/YUYVConvertedImagePi3\";\n\n// This function is called with messages from your input topics.\n// The first argument is an event with the topic, receive time, and message.\n// Use the `Input<...>` helper to get the correct event type for your input topic messages.\nexport default function script(\n event: Input<\"/pi3/camera/decimated frc971.vision.CameraImage\">\n): Message<\"foxglove_msgs/RawImage\"> {\n let timestampmt: Time;\n timestampmt = {\n sec: Number(event.message.monotonic_timestamp_ns) / 1000000000,\n nsec: Number(event.message.monotonic_timestamp_ns),\n };\n const data = new Uint8Array(event.message.data.length);\n for (let ii = 0; ii < data.length; ii += 4) {\n data[ii] = event.message.data[ii + 1];\n data[ii + 1] = event.message.data[ii];\n data[ii + 2] = event.message.data[ii + 3];\n data[ii + 3] = event.message.data[ii + 2];\n }\n return {\n timestamp: timestampmt,\n frame_id: \"PI3\",\n width: event.message.cols,\n height: event.message.rows,\n encoding: \"yuv422\",\n step: 1280,\n data: data,\n };\n}\n",
+ "name": "86094f4e"
+ },
+ "202f16df-82f5-48d8-8318-55a6bb305866": {
+ "sourceCode": "// The ./types module provides helper types for your Input events and messages.\nimport { Input, Message, Time } from \"./types\";\n\n// Your script can output well-known message types, any of your custom message types, or\n// complete custom message types.\n//\n// Use `Message` to access your data source types or well-known types:\n// type Twist = Message<\"geometry_msgs/Twist\">;\n\n// These are the topics your script \"subscribes\" to. Studio will invoke your script function\n// when any message is received on one of these topics.\nexport const inputs = [\"/pi4/camera/decimated frc971.vision.CameraImage\"];\n\n// Any output your script produces is \"published\" to this topic. Published messages are only visible within Studio, not to your original data source.\nexport const output = \"/studio_script/YUYVConvertedImagePi4\";\n\n// This function is called with messages from your input topics.\n// The first argument is an event with the topic, receive time, and message.\n// Use the `Input<...>` helper to get the correct event type for your input topic messages.\nexport default function script(\n event: Input<\"/pi4/camera/decimated frc971.vision.CameraImage\">\n): Message<\"foxglove_msgs/RawImage\"> {\n let timestampmt: Time;\n timestampmt = {\n sec: Number(event.message.monotonic_timestamp_ns) / 1000000000,\n nsec: Number(event.message.monotonic_timestamp_ns),\n };\n const data = new Uint8Array(event.message.data.length);\n for (let ii = 0; ii < data.length; ii += 4) {\n data[ii] = event.message.data[ii + 1];\n data[ii + 1] = event.message.data[ii];\n data[ii + 2] = event.message.data[ii + 3];\n data[ii + 3] = event.message.data[ii + 2];\n }\n return {\n timestamp: timestampmt,\n frame_id: \"PI4\",\n width: event.message.cols,\n height: event.message.rows,\n encoding: \"yuv422\",\n step: 1280,\n data: data,\n };\n}\n",
+ "name": "202f16df"
+ }
+ },
+ "playbackConfig": {
+ "speed": 1
+ },
+ "layout": {
+ "direction": "row",
+ "first": {
+ "first": "ImageViewPanel!3j0qvlm",
+ "second": "ImageViewPanel!1uof9zo",
+ "direction": "column"
+ },
+ "second": {
+ "direction": "row",
+ "first": {
+ "first": "ImageViewPanel!1ya4lw8",
+ "second": "ImageViewPanel!1tvn3cp",
+ "direction": "column"
+ },
+ "second": "ImageViewPanel!jmmyy1",
+ "splitPercentage": 40.642717427911904
+ },
+ "splitPercentage": 31.186958104147582
+ }
+}
\ No newline at end of file
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 2ddb735..5c98d7c 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -1,45 +1,4 @@
-py_binary(
- name = "create_calib_file",
- srcs = [
- "create_calib_file.py",
- ],
- args = [
- "calibration_data.h",
- ],
- data = glob(["calib_files/*.json"]),
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//frc971/vision:create_calib_file",
- ],
-)
-
-genrule(
- name = "run_calibration_data",
- outs = [
- "calibration_data.h",
- ],
- cmd = " ".join([
- "$(location :create_calib_file)",
- "$(location calibration_data.h)",
- ]),
- target_compatible_with = ["@platforms//os:linux"],
- tools = [
- ":create_calib_file",
- ],
-)
-
-cc_library(
- name = "calibration_data",
- hdrs = [
- "calibration_data.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "@com_google_absl//absl/types:span",
- ],
-)
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
cc_binary(
name = "camera_reader",
@@ -89,19 +48,38 @@
visibility = ["//y2023:__subpackages__"],
deps = [
":aprilrobotics_lib",
- ":calibration_data",
"//aos:init",
"//aos/events:simulated_event_loop",
"//aos/events/logging:log_reader",
+ "//frc971/constants:constants_sender_lib",
"//frc971/control_loops:pose",
"//frc971/vision:calibration_fbs",
"//frc971/vision:charuco_lib",
"//frc971/vision:target_mapper",
"//third_party:opencv",
+ "//y2023/constants:constants_fbs",
],
)
cc_library(
+ name = "vision_util",
+ srcs = ["vision_util.cc"],
+ hdrs = ["vision_util.h"],
+ deps = [
+ "//y2023/constants:constants_fbs",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "april_debug_fbs",
+ srcs = ["april_debug.fbs"],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
name = "aprilrobotics_lib",
srcs = [
"aprilrobotics.cc",
@@ -110,9 +88,11 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2023:__subpackages__"],
deps = [
- ":calibration_data",
+ ":april_debug_fbs",
+ ":vision_util",
"//aos:init",
"//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
"//frc971/vision:calibration_fbs",
"//frc971/vision:charuco_lib",
"//frc971/vision:target_map_fbs",
@@ -120,6 +100,7 @@
"//frc971/vision:vision_fbs",
"//third_party:opencv",
"//third_party/apriltag",
+ "//y2023/constants:constants_fbs",
],
)
diff --git a/y2023/vision/april_debug.fbs b/y2023/vision/april_debug.fbs
new file mode 100644
index 0000000..4442448
--- /dev/null
+++ b/y2023/vision/april_debug.fbs
@@ -0,0 +1,21 @@
+namespace y2023.vision;
+
+// Stores image xy pixel coordinates of apriltag corners
+struct Point {
+ x:double (id: 0);
+ y:double (id: 1);
+}
+
+// Corner locations for each apriltag
+table AprilCorners {
+ id:uint64 (id: 0);
+ // Will always have 4 values, one for each corner
+ points: [Point] (id: 1);
+}
+
+// List of positions of all apriltags detected in current frame
+table AprilDebug {
+ corners: [AprilCorners] (id: 0);
+}
+
+root_type AprilDebug;
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index cbfd3f6..cb6a5bc 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -1,11 +1,11 @@
#include "y2023/vision/aprilrobotics.h"
+#include "y2023/vision/vision_util.h"
+
DEFINE_bool(
debug, false,
"If true, dump a ton of debug and crash on the first valid detection.");
-DEFINE_int32(team_number, 971,
- "Use the calibration for a node with this team number");
namespace y2023 {
namespace vision {
@@ -13,7 +13,7 @@
AprilRoboticsDetector::AprilRoboticsDetector(aos::EventLoop *event_loop,
std::string_view channel_name)
- : calibration_data_(CalibrationData()),
+ : calibration_data_(event_loop),
ftrace_(),
image_callback_(
event_loop, channel_name,
@@ -23,7 +23,9 @@
},
chrono::milliseconds(5)),
target_map_sender_(
- event_loop->MakeSender<frc971::vision::TargetMap>("/camera")) {
+ event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
+ april_debug_sender_(
+ event_loop->MakeSender<y2023::vision::AprilDebug>("/camera")) {
tag_family_ = tag16h5_create();
tag_detector_ = apriltag_detector_create();
@@ -36,14 +38,8 @@
std::string hostname = aos::network::GetHostname();
// Check team string is valid
- std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(hostname);
- std::optional<uint16_t> team_number =
- aos::network::team_number_internal::ParsePiTeamNumber(hostname);
- CHECK(pi_number) << "Unable to parse pi number from '" << hostname << "'";
- CHECK(team_number);
-
- calibration_ = FindCameraCalibration(&calibration_data_.message(),
- "pi" + std::to_string(*pi_number));
+ calibration_ = FindCameraCalibration(
+ calibration_data_.constants(), event_loop->node()->name()->string_view());
intrinsics_ = CameraIntrinsics(calibration_);
camera_distortion_coeffs_ = CameraDistCoeffs(calibration_);
@@ -124,6 +120,10 @@
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
+ std::vector<flatbuffers::Offset<AprilCorners>> corners_vector;
+
+ auto builder = april_debug_sender_.MakeBuilder();
+
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
zarray_get(detections, i, &det);
@@ -159,9 +159,35 @@
before_pose_estimation)
.count()
<< " seconds for pose estimation";
+
+ std::vector<Point> corner_points;
+
+ corner_points.emplace_back(det->p[0][0], det->p[0][1]);
+ corner_points.emplace_back(det->p[1][0], det->p[1][1]);
+ corner_points.emplace_back(det->p[2][0], det->p[2][1]);
+ corner_points.emplace_back(det->p[3][0], det->p[3][1]);
+
+ auto corner_points_fbs =
+ builder.fbb()->CreateVectorOfStructs(corner_points);
+
+ AprilCorners::Builder april_corners_builder =
+ builder.MakeBuilder<AprilCorners>();
+
+ april_corners_builder.add_id(det->id);
+ april_corners_builder.add_points(corner_points_fbs);
+
+ corners_vector.emplace_back(april_corners_builder.Finish());
}
}
+ auto corners_vector_fbs = builder.fbb()->CreateVector(corners_vector);
+
+ AprilDebug::Builder april_debug_builder = builder.MakeBuilder<AprilDebug>();
+
+ april_debug_builder.add_corners(corners_vector_fbs);
+
+ builder.CheckOk(builder.Send(april_debug_builder.Finish()));
+
apriltag_detections_destroy(detections);
const aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index a68b1d9..8c2c0ad 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -15,7 +15,9 @@
#include "third_party/apriltag/apriltag.h"
#include "third_party/apriltag/apriltag_pose.h"
#include "third_party/apriltag/tag16h5.h"
-#include "y2023/vision/calibration_data.h"
+#include "y2023/vision/april_debug_generated.h"
+#include "y2023/constants/constants_generated.h"
+#include "frc971/constants/constants_sender_lib.h"
DECLARE_int32(team_number);
@@ -42,23 +44,6 @@
frc971::vision::TargetMapper::TargetId target_id,
flatbuffers::FlatBufferBuilder *fbb);
- static const frc971::vision::calibration::CameraCalibration *
- FindCameraCalibration(
- const frc971::vision::calibration::CalibrationData *calibration_data,
- std::string_view node_name) {
- for (const frc971::vision::calibration::CameraCalibration *candidate :
- *calibration_data->camera_calibrations()) {
- if (candidate->node_name()->string_view() != node_name) {
- continue;
- }
- if (candidate->team_number() != FLAGS_team_number) {
- continue;
- }
- return candidate;
- }
- LOG(FATAL) << ": Failed to find camera calibration for " << node_name
- << " on " << FLAGS_team_number;
- }
static cv::Mat CameraIntrinsics(
const frc971::vision::calibration::CameraCalibration
@@ -85,8 +70,7 @@
apriltag_family_t *tag_family_;
apriltag_detector_t *tag_detector_;
- const aos::FlatbufferSpan<frc971::vision::calibration::CalibrationData>
- calibration_data_;
+ const frc971::constants::ConstantsFetcher<Constants> calibration_data_;
const frc971::vision::calibration::CameraCalibration *calibration_;
cv::Mat intrinsics_;
cv::Mat camera_distortion_coeffs_;
@@ -95,6 +79,7 @@
frc971::vision::ImageCallback image_callback_;
aos::Sender<frc971::vision::TargetMap> target_map_sender_;
+ aos::Sender<y2023::vision::AprilDebug> april_debug_sender_;
};
} // namespace vision
diff --git a/y2023/vision/aprilrobotics_main.cc b/y2023/vision/aprilrobotics_main.cc
index 67b853f..6870ae1 100644
--- a/y2023/vision/aprilrobotics_main.cc
+++ b/y2023/vision/aprilrobotics_main.cc
@@ -1,6 +1,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "y2023/vision/aprilrobotics.h"
+#include "frc971/constants/constants_sender_lib.h"
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
@@ -9,6 +10,8 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig(FLAGS_config);
+ frc971::constants::WaitForConstants<Constants>(&config.message());
+
aos::ShmEventLoop event_loop(&config.message());
AprilRoboticsDetector detector(&event_loop, "/camera");
diff --git a/y2023/vision/calib_files/BUILD b/y2023/vision/calib_files/BUILD
new file mode 100644
index 0000000..1f33d50
--- /dev/null
+++ b/y2023/vision/calib_files/BUILD
@@ -0,0 +1,5 @@
+filegroup(
+ name = "calib_files",
+ srcs = glob(["*.json"]),
+ visibility = ["//visibility:public"],
+)
diff --git a/y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json b/y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json
old mode 100755
new mode 100644
index a8d8816..fc739fd
--- a/y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json
+++ b/y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json
@@ -1,41 +1,43 @@
{
- "node_name": "pi1",
- "team_number": 7971,
- "intrinsics": [
- 388.369812,
- 0.0,
- 292.325653,
- 0.0,
- 388.513733,
- 224.371063,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- 0.126935,
- -0.218447,
- -0.000152,
- 0.001158,
- 0.06266
- ],
- "fixed_extrinsics": [
- -1.0,
- -1.57586107256918e-16,
- 5.0158596452676243e-17,
- -0.15239999999999998,
- 1.3147519464173305e-16,
- -0.5735764363510459,
- 0.8191520442889919,
- -0.2032,
- -1.0031719290535249e-16,
- 0.8191520442889919,
- 0.5735764363510459,
- 0.0127,
- 0.0,
- 0.0,
- 0.0,
- 1.0
- ],
- "calibration_timestamp": 1623537339636386620
+ "node_name": "pi1",
+ "team_number": 7971,
+ "intrinsics": [
+ 388.369812,
+ 0,
+ 292.325653,
+ 0,
+ 388.513733,
+ 224.371063,
+ 0,
+ 0,
+ 1
+ ],
+ "dist_coeffs": [
+ 0.126935,
+ -0.218447,
+ -0.000152,
+ 0.001158,
+ 0.06266
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ -1,
+ -1.57586107256918e-16,
+ 5.0158596452676243e-17,
+ -0.15239999999999998,
+ 1.3147519464173305e-16,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ -0.2032,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0,
+ 0,
+ 0,
+ 1
+ ]
+ },
+ "calibration_timestamp": 1623537339636386600
}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json b/y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json
old mode 100755
new mode 100644
index d994a07..3cf285c
--- a/y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json
+++ b/y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json
@@ -1,41 +1,43 @@
{
- "node_name": "pi2",
- "team_number": 7971,
- "intrinsics": [
- 388.7565,
- 0.0,
- 285.024506,
- 0.0,
- 388.915039,
- 222.227539,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- 0.128415,
- -0.212528,
- 0.001165,
- 0.000579,
- 0.054853
- ],
- "fixed_extrinsics": [
- 7.02428546843654e-17,
- -0.5735764363510459,
- 0.8191520442889919,
- 0.09525,
- 1.0,
- 1.2246467991473532e-16,
- 0.0,
- 0.1905,
- -1.0031719290535249e-16,
- 0.8191520442889919,
- 0.5735764363510459,
- 0.0127,
- 0.0,
- 0.0,
- 0.0,
- 1.0
- ],
- "calibration_timestamp": 1623537020325393444
+ "node_name": "pi2",
+ "team_number": 7971,
+ "intrinsics": [
+ 388.7565,
+ 0,
+ 285.024506,
+ 0,
+ 388.915039,
+ 222.227539,
+ 0,
+ 0,
+ 1
+ ],
+ "dist_coeffs": [
+ 0.128415,
+ -0.212528,
+ 0.001165,
+ 0.000579,
+ 0.054853
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 7.02428546843654e-17,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ 0.09525,
+ 1,
+ 1.2246467991473532e-16,
+ 0,
+ 0.1905,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0,
+ 0,
+ 0,
+ 1
+ ]
+ },
+ "calibration_timestamp": 1623537020325393400
}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json b/y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json
old mode 100755
new mode 100644
index 241957e..c7a7581
--- a/y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json
+++ b/y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json
@@ -1,41 +1,43 @@
{
- "node_name": "pi3",
- "team_number": 7971,
- "intrinsics": [
- 389.35611,
- 0.0,
- 339.345673,
- 0.0,
- 389.516235,
- 240.247787,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- 0.122511,
- -0.209383,
- -0.001212,
- 0.000041,
- 0.05674
- ],
- "fixed_extrinsics": [
- 7.02428546843654e-17,
- -0.5735764363510459,
- 0.8191520442889919,
- 0.09525,
- 1.0,
- 1.2246467991473532e-16,
- 0.0,
- -0.10794999999999999,
- -1.0031719290535249e-16,
- 0.8191520442889919,
- 0.5735764363510459,
- 0.0127,
- 0.0,
- 0.0,
- 0.0,
- 1.0
- ],
- "calibration_timestamp": 1623537211977365877
+ "node_name": "pi3",
+ "team_number": 7971,
+ "intrinsics": [
+ 389.35611,
+ 0,
+ 339.345673,
+ 0,
+ 389.516235,
+ 240.247787,
+ 0,
+ 0,
+ 1
+ ],
+ "dist_coeffs": [
+ 0.122511,
+ -0.209383,
+ -0.001212,
+ 4.1e-05,
+ 0.05674
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 7.02428546843654e-17,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ 0.09525,
+ 1,
+ 1.2246467991473532e-16,
+ 0,
+ -0.10794999999999999,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0,
+ 0,
+ 0,
+ 1
+ ]
+ },
+ "calibration_timestamp": 1623537211977365800
}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json b/y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json
old mode 100755
new mode 100644
index 6e04089..f8f6b1f
--- a/y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json
+++ b/y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json
@@ -1,41 +1,43 @@
{
- "node_name": "pi4",
- "team_number": 7971,
- "intrinsics": [
- 390.301514,
- 0.0,
- 356.104095,
- 0.0,
- 389.884491,
- 231.157303,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- 0.128595,
- -0.229324,
- -0.001145,
- 0.001602,
- 0.079774
- ],
- "fixed_extrinsics": [
- 7.02428546843654e-17,
- -0.5735764363510459,
- 0.8191520442889919,
- -0.15239999999999998,
- 1.0,
- 1.2246467991473532e-16,
- 0.0,
- -0.17779999999999999,
- -1.0031719290535249e-16,
- 0.8191520442889919,
- 0.5735764363510459,
- 0.0127,
- 0.0,
- 0.0,
- 0.0,
- 1.0
- ],
- "calibration_timestamp": 1623537445706564865
+ "node_name": "pi4",
+ "team_number": 7971,
+ "intrinsics": [
+ 390.301514,
+ 0,
+ 356.104095,
+ 0,
+ 389.884491,
+ 231.157303,
+ 0,
+ 0,
+ 1
+ ],
+ "dist_coeffs": [
+ 0.128595,
+ -0.229324,
+ -0.001145,
+ 0.001602,
+ 0.079774
+ ],
+ "fixed_extrinsics": {
+ "data": [
+ 7.02428546843654e-17,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ -0.15239999999999998,
+ 1,
+ 1.2246467991473532e-16,
+ 0,
+ -0.17779999999999999,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0,
+ 0,
+ 0,
+ 1
+ ]
+ },
+ "calibration_timestamp": 1623537445706564900
}
diff --git a/y2023/vision/create_calib_file.py b/y2023/vision/create_calib_file.py
deleted file mode 100644
index b5e620d..0000000
--- a/y2023/vision/create_calib_file.py
+++ /dev/null
@@ -1,4 +0,0 @@
-import frc971.vision.create_calib_file
-
-if __name__ == "__main__":
- frc971.vision.create_calib_file.generate_header("2023")
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 6d32dbd..4f10499 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -13,7 +13,7 @@
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc.hpp"
#include "y2023/vision/aprilrobotics.h"
-#include "y2023/vision/calibration_data.h"
+#include "y2023/vision/vision_util.h"
DEFINE_string(json_path, "target_map.json",
"Specify path for json with initial pose guesses.");
@@ -21,7 +21,6 @@
namespace y2023 {
namespace vision {
-using frc971::vision::CharucoExtractor;
using frc971::vision::DataAdapter;
using frc971::vision::ImageCallback;
using frc971::vision::PoseUtils;
@@ -76,23 +75,6 @@
}
}
-const calibration::CameraCalibration *FindCameraCalibration(
- const calibration::CalibrationData *calibration_data,
- std::string_view node_name) {
- for (const calibration::CameraCalibration *candidate :
- *calibration_data->camera_calibrations()) {
- if (candidate->node_name()->string_view() != node_name) {
- continue;
- }
- if (candidate->team_number() != FLAGS_team_number) {
- continue;
- }
- return candidate;
- }
- LOG(FATAL) << ": Failed to find camera calibration for " << node_name
- << " on " << FLAGS_team_number;
-}
-
Eigen::Affine3d CameraExtrinsics(
const calibration::CameraCalibration *camera_calibration) {
const frc971::vision::calibration::TransformationMatrix *transform =
@@ -113,16 +95,15 @@
// Get images from pi and pass apriltag positions to HandleAprilTag()
void HandlePiCaptures(
+ const frc971::constants::ConstantsFetcher<Constants> &constants,
aos::EventLoop *pi_event_loop, aos::logger::LogReader *reader,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors) {
- const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
- CalibrationData());
const auto node_name = pi_event_loop->node()->name()->string_view();
const calibration::CameraCalibration *calibration =
- FindCameraCalibration(&calibration_data.message(), node_name);
+ FindCameraCalibration(constants.constants(), node_name);
const auto extrinsics = CameraExtrinsics(calibration);
// TODO(milind): change to /camera once we log at full frequency
@@ -158,28 +139,36 @@
aos::configuration::GetNode(reader.configuration(), "pi1");
std::unique_ptr<aos::EventLoop> pi1_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi1", pi1);
- HandlePiCaptures(pi1_event_loop.get(), &reader,
+ frc971::constants::ConstantsFetcher<Constants> pi1_constants(
+ pi1_event_loop.get());
+ HandlePiCaptures(pi1_constants, pi1_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
const aos::Node *pi2 =
aos::configuration::GetNode(reader.configuration(), "pi2");
std::unique_ptr<aos::EventLoop> pi2_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi2", pi2);
- HandlePiCaptures(pi2_event_loop.get(), &reader,
+ frc971::constants::ConstantsFetcher<Constants> pi2_constants(
+ pi2_event_loop.get());
+ HandlePiCaptures(pi2_constants, pi2_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
const aos::Node *pi3 =
aos::configuration::GetNode(reader.configuration(), "pi3");
std::unique_ptr<aos::EventLoop> pi3_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi3", pi3);
- HandlePiCaptures(pi3_event_loop.get(), &reader,
+ frc971::constants::ConstantsFetcher<Constants> pi3_constants(
+ pi3_event_loop.get());
+ HandlePiCaptures(pi3_constants, pi3_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
const aos::Node *pi4 =
aos::configuration::GetNode(reader.configuration(), "pi4");
std::unique_ptr<aos::EventLoop> pi4_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi4", pi4);
- HandlePiCaptures(pi4_event_loop.get(), &reader,
+ frc971::constants::ConstantsFetcher<Constants> pi4_constants(
+ pi4_event_loop.get());
+ HandlePiCaptures(pi4_constants, pi4_event_loop.get(), &reader,
×tamped_target_detections, &detectors);
reader.event_loop_factory()->Run();
diff --git a/y2023/vision/vision_util.cc b/y2023/vision/vision_util.cc
new file mode 100644
index 0000000..eed315a
--- /dev/null
+++ b/y2023/vision/vision_util.cc
@@ -0,0 +1,19 @@
+#include "y2023/vision/vision_util.h"
+
+#include "glog/logging.h"
+
+namespace y2023::vision {
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2023::Constants &calibration_data, std::string_view node_name) {
+ CHECK(calibration_data.has_cameras());
+ for (const y2023::CameraConfiguration *candidate :
+ *calibration_data.cameras()) {
+ CHECK(candidate->has_calibration());
+ if (candidate->calibration()->node_name()->string_view() != node_name) {
+ continue;
+ }
+ return candidate->calibration();
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name;
+}
+} // namespace y2023::vision
diff --git a/y2023/vision/vision_util.h b/y2023/vision/vision_util.h
new file mode 100644
index 0000000..58f9a7f
--- /dev/null
+++ b/y2023/vision/vision_util.h
@@ -0,0 +1,11 @@
+#ifndef Y2023_VISION_VISION_UTIL_H_
+#define Y2023_VISION_VISION_UTIL_H_
+#include <string_view>
+
+#include "y2023/constants/constants_generated.h"
+namespace y2023::vision {
+
+const frc971::vision::calibration::CameraCalibration *FindCameraCalibration(
+ const y2023::Constants &calibration_data, std::string_view node_name);
+}
+#endif // Y2023_VISION_VISION_UTIL_H_
diff --git a/y2023/y2023_imu.json b/y2023/y2023_imu.json
index 7744281..9636003 100644
--- a/y2023/y2023_imu.json
+++ b/y2023/y2023_imu.json
@@ -298,12 +298,21 @@
"frequency": 2200,
"max_size": 1600,
"num_senders": 2
+ },
+ {
+ "name": "/imu/constants",
+ "type": "y2023.Constants",
+ "source_node": "imu",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 4096
}
],
"applications": [
{
"name": "message_bridge_client",
"executable_name": "message_bridge_client.sh",
+ "user": "pi",
"nodes": [
"imu"
]
@@ -313,6 +322,7 @@
"executable_name": "localizer_main",
/* TODO(james): Remove this once confident in the accelerometer code. */
"args": ["--ignore_accelerometer"],
+ "user": "pi",
"nodes": [
"imu"
]
@@ -320,6 +330,7 @@
{
"name": "imu",
"executable_name": "imu_main",
+ "user": "pi",
"nodes": [
"imu"
]
@@ -327,6 +338,7 @@
{
"name": "message_bridge_server",
"executable_name": "message_bridge_server",
+ "user": "pi",
"nodes": [
"imu"
]
@@ -335,6 +347,7 @@
"name": "localizer_logger",
"executable_name": "logger_main",
"args": ["--logging_folder", "", "--snappy_compress"],
+ "user": "pi",
"nodes": [
"imu"
]
@@ -342,6 +355,15 @@
{
"name": "web_proxy",
"executable_name": "web_proxy_main",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "user": "pi",
"nodes": [
"imu"
]
@@ -350,6 +372,15 @@
"maps": [
{
"match": {
+ "name": "/constants*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/constants"
+ }
+ },
+ {
+ "match": {
"name": "/aos*",
"source_node": "imu"
},
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index cdbd8ec..0f1e633 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -402,6 +402,14 @@
"time_to_live": 500000000
}
]
+ },
+ {
+ "name": "/logger/constants",
+ "type": "y2023.Constants",
+ "source_node": "logger",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 4096
}
],
"maps": [
@@ -416,6 +424,15 @@
},
{
"match": {
+ "name": "/constants*",
+ "source_node": "logger"
+ },
+ "rename": {
+ "name": "/logger/constants"
+ }
+ },
+ {
+ "match": {
"name": "/camera*",
"source_node": "logger"
},
@@ -461,12 +478,12 @@
"executable_name": "logger_main",
"autostart": false,
"args": [
- "--snappy_compress",
"--logging_folder",
"",
- "--snappy_compress",
"--rotate_every",
- "60.0"
+ "60.0",
+ "--direct",
+ "--flush_size=4194304"
],
"nodes": [
"logger"
@@ -479,6 +496,13 @@
"nodes": [
"logger"
]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "nodes": [
+ "logger"
+ ]
}
],
"nodes": [
@@ -488,7 +512,9 @@
"hostnames": [
"pi-971-6",
"pi-9971-6",
+ "pi-7971-6",
"ASchuh-T480s",
+ "tarvalon",
"aschuh-3950x"
],
"port": 9971
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index e641d5d..c722110 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -203,6 +203,15 @@
]
},
{
+ "name": "/pi{{ NUM }}/camera",
+ "type": "y2023.vision.AprilDebug",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 40,
+ "num_senders": 2,
+ "max_size": 40000,
+ "logger": "LOCAL_LOGGER"
+ },
+ {
"name": "/pi{{ NUM }}/aos/remote_timestamps/imu/pi{{ NUM }}/camera/frc971-vision-TargetMap",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 80,
@@ -315,6 +324,14 @@
"frequency": 50,
"num_senders": 2,
"max_size": 200
+ },
+ {
+ "name": "/pi{{ NUM }}/constants",
+ "type": "y2023.Constants",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 4096
}
],
"applications": [
@@ -362,6 +379,22 @@
]
},
{
+ "name": "image_logger",
+ "executable_name": "logger_main",
+ "autostart": false,
+ "args": [
+ "--logging_folder",
+ "",
+ "--rotate_every",
+ "60.0",
+ "--direct",
+ "--flush_size=4194304"
+ ],
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
+ },
+ {
"name": "aprilrobotics",
"executable_name": "aprilrobotics",
"args": ["--enable_ftrace"],
@@ -369,6 +402,14 @@
"nodes": [
"pi{{ NUM }}"
]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "user": "pi",
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
}
],
"maps": [
@@ -383,6 +424,15 @@
},
{
"match": {
+ "name": "/constants*",
+ "source_node": "pi{{ NUM }}"
+ },
+ "rename": {
+ "name": "/pi{{ NUM }}/constants"
+ }
+ },
+ {
+ "match": {
"name": "/camera*",
"source_node": "pi{{ NUM }}"
},
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index e1306c2..d0b2251 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -435,6 +435,14 @@
"type": "frc971.wpilib.PneumaticsToLog",
"source_node": "roborio",
"frequency": 50
+ },
+ {
+ "name": "/roborio/constants",
+ "type": "y2023.Constants",
+ "source_node": "roborio",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 4096
}
],
"applications": [
@@ -511,11 +519,27 @@
"nodes": [
"roborio"
]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "nodes": [
+ "roborio"
+ ]
}
],
"maps": [
{
"match": {
+ "name": "/constants*",
+ "source_node": "roborio"
+ },
+ "rename": {
+ "name": "/roborio/constants"
+ }
+ },
+ {
+ "match": {
"name": "/aos*",
"source_node": "roborio"
},