Merge "Dewarp refined tag edges"
diff --git a/WORKSPACE b/WORKSPACE
index b00de63..752cdf8 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -1433,6 +1433,14 @@
)
http_archive(
+ name = "com_github_nghttp2_nghttp2",
+ build_file = "//debian:BUILD.nghttp2.bazel",
+ sha256 = "7da19947b33a07ddcf97b9791331bfee8a8545e6b394275a9971f43cae9d636b",
+ strip_prefix = "nghttp2-1.58.0",
+ url = "https://github.com/nghttp2/nghttp2/archive/refs/tags/v1.58.0.tar.gz",
+)
+
+http_archive(
# No official name exists. Names used in our external dependencies include
# zlib, madler_zlib, com_github_madler_zlib.
name = "zlib",
diff --git a/aos/BUILD b/aos/BUILD
index 4d041ca..7c0ad3d 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -239,6 +239,7 @@
srcs = ["configuration.fbs"],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
+ deps = ["//aos/flatbuffers/reflection:reflection_fbs"],
)
cc_static_flatbuffer(
diff --git a/aos/configuration.fbs b/aos/configuration.fbs
index 0b42f5b..b2b34c1 100644
--- a/aos/configuration.fbs
+++ b/aos/configuration.fbs
@@ -169,6 +169,10 @@
// If set, this is the memory limit to enforce in bytes for the application
// (and it's children)
memory_limit:uint64 = 0 (id: 8);
+
+ // If set, this is the number of nanoseconds the application has to stop. If the application
+ // doesn't stop within the specified time, then it is killed.
+ stop_time:int64 = 1000000000 (id: 9);
}
// Per node data and connection information.
diff --git a/aos/events/logging/log_backend_test.cc b/aos/events/logging/log_backend_test.cc
index d3c83cc..1e95c10 100644
--- a/aos/events/logging/log_backend_test.cc
+++ b/aos/events/logging/log_backend_test.cc
@@ -128,7 +128,17 @@
TEST(QueueAlignmentTest, Cases) {
QueueAligner aligner;
- uint8_t *start = nullptr;
+
+ // Get a 512-byte-aligned pointer to a buffer. That buffer needs to be at
+ // least 3 sectors big for the purposes of this test.
+ uint8_t buffer[FileHandler::kSector * 4];
+ void *aligned_start = buffer;
+ size_t size = sizeof(buffer);
+ ASSERT_TRUE(std::align(FileHandler::kSector, FileHandler::kSector * 3,
+ aligned_start, size) != nullptr);
+ ASSERT_GE(size, FileHandler::kSector * 3);
+
+ uint8_t *start = static_cast<uint8_t *>(aligned_start);
{
// Only prefix
std::vector<absl::Span<const uint8_t>> queue;
diff --git a/aos/events/logging/logfile_utils_out_of_space_test.sh b/aos/events/logging/logfile_utils_out_of_space_test.sh
index f412e0d..6a32347 100755
--- a/aos/events/logging/logfile_utils_out_of_space_test.sh
+++ b/aos/events/logging/logfile_utils_out_of_space_test.sh
@@ -23,7 +23,7 @@
rm -rf "${TMPFS}"
mkdir "${TMPFS}"
-function test {
+function run_test {
SIZE="$1"
echo "Running test with ${SIZE}..." >&2
unshare --mount --map-root-user bash <<END
@@ -37,10 +37,10 @@
}
# Run out of space exactly at the beginning of a block.
-test 81920
+run_test 81920
# Run out of space 1 byte into a block.
-test 81921
+run_test 81921
# Run out of space in the middle of a block.
-test 87040
+run_test 87040
diff --git a/aos/events/logging/multinode_logger_test_lib.h b/aos/events/logging/multinode_logger_test_lib.h
index 63604d6..8f64f66 100644
--- a/aos/events/logging/multinode_logger_test_lib.h
+++ b/aos/events/logging/multinode_logger_test_lib.h
@@ -76,13 +76,13 @@
};
constexpr std::string_view kCombinedConfigSha1() {
- return "32514f3a686e5f8936cc4651e7c81350112f7be8d80dcb8d4afaa29d233c5619";
+ return "71eb8341221fbabefb4ddde43bcebf794fd5855e3ad77786a1db0f9e27a39091";
}
constexpr std::string_view kSplitConfigSha1() {
- return "416da222c09d83325c6f453591d34c7ef12c12c2dd129ddeea657c4bec61b7fd";
+ return "f61d45dc0bda026e852e2da9b3e5c2c7f1c89c9f7958cfba3d02e2c960416f04";
}
constexpr std::string_view kReloggedSplitConfigSha1() {
- return "3fe428684a38298d3323ef087f44517574da3f07dd84b3740829156d6d870108";
+ return "3d8fd3d13955b517ee3d66a50b5e4dd7a13fd648f469d16910990418bcfc6beb";
}
LoggerState MakeLoggerState(NodeEventLoopFactory *node,
diff --git a/aos/flatbuffers.h b/aos/flatbuffers.h
index a299fe9..607303c 100644
--- a/aos/flatbuffers.h
+++ b/aos/flatbuffers.h
@@ -118,6 +118,11 @@
// make attempts to use it fail more obviously.
void Wipe() { memset(span().data(), 0, span().size()); }
+ // Returns true if the flatbuffer is valid. Returns false if either:
+ // * The flatbuffer is incorrectly constructed (e.g., it points to memory
+ // locations outside of the current memory buffer).
+ // * The flatbuffer is too complex, and the flatbuffer verifier chosen to bail
+ // when attempting to traverse the tree of tables.
bool Verify() const {
if (span().size() < 4u) {
return false;
diff --git a/aos/flatbuffers/BUILD b/aos/flatbuffers/BUILD
index 08d548f..32f1d39 100644
--- a/aos/flatbuffers/BUILD
+++ b/aos/flatbuffers/BUILD
@@ -102,6 +102,7 @@
":test_schema",
"//aos:flatbuffers",
"//aos:json_to_flatbuffer",
+ "//aos/flatbuffers/test_dir:include_reflection_fbs",
"//aos/flatbuffers/test_dir:type_coverage_fbs",
"//aos/testing:googletest",
"//aos/testing:path",
diff --git a/aos/flatbuffers/builder.h b/aos/flatbuffers/builder.h
index db89d10..36225c0 100644
--- a/aos/flatbuffers/builder.h
+++ b/aos/flatbuffers/builder.h
@@ -77,6 +77,9 @@
FlatbufferSpan<typename T::Flatbuffer> AsFlatbufferSpan() {
return {buffer()};
}
+ FlatbufferSpan<const typename T::Flatbuffer> AsFlatbufferSpan() const {
+ return {buffer()};
+ }
// Returns true if the flatbuffer is validly constructed. Should always return
// true (barring some sort of memory corruption). Exposed for convenience.
diff --git a/aos/flatbuffers/reflection/BUILD.bazel b/aos/flatbuffers/reflection/BUILD.bazel
new file mode 100644
index 0000000..475f2a2
--- /dev/null
+++ b/aos/flatbuffers/reflection/BUILD.bazel
@@ -0,0 +1,18 @@
+load("@aspect_bazel_lib//lib:copy_file.bzl", "copy_file")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+
+copy_file(
+ name = "reflection_fbs_copy",
+ src = "@com_github_google_flatbuffers//reflection:reflection_fbs_schema",
+ out = "reflection.fbs",
+)
+
+# This autogenerates both a reflection_static.h and a reflection_generated.h.
+# However, in order to avoid having two conflicting headers floating around,
+# we forcibly override the #include to use flatbuffers/reflection_generated.h
+# in static_flatbuffers.cc
+static_flatbuffer(
+ name = "reflection_fbs",
+ srcs = ["reflection.fbs"],
+ visibility = ["//visibility:public"],
+)
diff --git a/aos/flatbuffers/static_flatbuffers.cc b/aos/flatbuffers/static_flatbuffers.cc
index 8d8942c..c1b617f 100644
--- a/aos/flatbuffers/static_flatbuffers.cc
+++ b/aos/flatbuffers/static_flatbuffers.cc
@@ -19,6 +19,8 @@
bool is_inline = true;
// Whether this is a struct or not.
bool is_struct = false;
+ // Whether this is a repeated type (vector or string).
+ bool is_repeated = false;
// Full C++ type of this field.
std::string full_type = "";
// Full flatbuffer type for this field.
@@ -118,6 +120,22 @@
const std::string IncludePathForFbs(
std::string_view fbs_file, std::string_view include_suffix = "static") {
+ // Special case for the reflection_generated.h, which is checked into the
+ // repo.
+ // Note that we *do* autogenerated the reflection_static.h but that because
+ // it uses a special import path, we end up overriding the include anyways
+ // (note that we could muck around with the paths on the bazel side to instead
+ // get a cc_library with the correct include paths specified, although it is
+ // not clear that that would be any simpler than the extra else-if).
+ if (fbs_file == "reflection/reflection.fbs") {
+ if (include_suffix == "generated") {
+ return "flatbuffers/reflection_generated.h";
+ } else if (include_suffix == "static") {
+ return "aos/flatbuffers/reflection/reflection_static.h";
+ } else {
+ LOG(FATAL) << "This should be unreachable.";
+ }
+ }
fbs_file.remove_suffix(4);
return absl::StrCat(fbs_file, "_", include_suffix, ".h");
}
@@ -151,12 +169,14 @@
// straightforwards.
field->is_inline = true;
field->is_struct = false;
+ field->is_repeated = false;
field->full_type =
ScalarOrEnumType(schema, type->base_type(), type->index());
return;
case reflection::BaseType::String: {
field->is_inline = false;
field->is_struct = false;
+ field->is_repeated = true;
field->full_type =
absl::StrFormat("::aos::fbs::String<%d>",
GetLengthAttributeOrZero(field_fbs, "static_length"));
@@ -166,6 +186,7 @@
// We need to extract the name of the elements of the vector.
std::string element_type;
bool elements_are_inline = true;
+ field->is_repeated = true;
if (type->base_type() == reflection::BaseType::Vector) {
switch (type->element()) {
case reflection::BaseType::Obj: {
@@ -207,6 +228,7 @@
const reflection::Object *object = GetObject(schema, type->index());
field->is_inline = object->is_struct();
field->is_struct = object->is_struct();
+ field->is_repeated = false;
const std::string flatbuffer_name =
FlatbufferNameToCppName(object->name()->string_view());
if (field->is_inline) {
@@ -437,27 +459,97 @@
absl::StrJoin(clearers, "\n"));
}
+// Creates the FromFlatbuffer() method that copies from a flatbuffer object API
+// object (i.e., the FlatbufferT types).
+std::string MakeObjectCopier(const std::vector<FieldData> &fields) {
+ std::vector<std::string> copiers;
+ for (const FieldData &field : fields) {
+ if (field.is_struct) {
+ // Structs are stored as unique_ptr<FooStruct>
+ copiers.emplace_back(absl::StrFormat(R"code(
+ if (other.%s) {
+ set_%s(*other.%s);
+ }
+ )code",
+ field.name, field.name, field.name));
+ } else if (field.is_inline) {
+ // Inline non-struct elements are stored as FooType.
+ copiers.emplace_back(absl::StrFormat(R"code(
+ set_%s(other.%s);
+ )code",
+ field.name, field.name));
+ } else if (field.is_repeated) {
+ // strings are stored as std::string's.
+ // vectors are stored as std::vector's.
+ copiers.emplace_back(absl::StrFormat(R"code(
+ // Unconditionally copy strings/vectors, even if it will just end up
+ // being 0-length (this maintains consistency with the flatbuffer Pack()
+ // behavior).
+ if (!CHECK_NOTNULL(add_%s())->FromFlatbuffer(other.%s)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+ )code",
+ field.name, field.name));
+ } else {
+ // Tables are stored as unique_ptr<FooTable>
+ copiers.emplace_back(absl::StrFormat(R"code(
+ if (other.%s) {
+ if (!CHECK_NOTNULL(add_%s())->FromFlatbuffer(*other.%s)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+ }
+ )code",
+ field.name, field.name, field.name));
+ }
+ }
+ return absl::StrFormat(
+ R"code(
+ // Copies the contents of the provided flatbuffer into this flatbuffer,
+ // returning true on success.
+ // Because the Flatbuffer Object API does not provide any concept of an
+ // optionally populated scalar field, all scalar fields will be populated
+ // after a call to FromFlatbufferObject().
+ // This is a deep copy, and will call FromFlatbufferObject on
+ // any constituent objects.
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer::NativeTableType &other) {
+ Clear();
+ %s
+ return true;
+ }
+ [[nodiscard]] bool FromFlatbuffer(const flatbuffers::unique_ptr<Flatbuffer::NativeTableType>& other) {
+ return FromFlatbuffer(*other);
+ }
+)code",
+ absl::StrJoin(copiers, "\n"));
+}
+
+// Creates the FromFlatbuffer() method that copies from an actual flatbuffer
+// object.
std::string MakeCopier(const std::vector<FieldData> &fields) {
std::vector<std::string> copiers;
for (const FieldData &field : fields) {
if (field.is_struct) {
copiers.emplace_back(absl::StrFormat(R"code(
- if (other->has_%s()) {
- set_%s(*other->%s());
+ if (other.has_%s()) {
+ set_%s(*other.%s());
}
)code",
field.name, field.name, field.name));
} else if (field.is_inline) {
copiers.emplace_back(absl::StrFormat(R"code(
- if (other->has_%s()) {
- set_%s(other->%s());
+ if (other.has_%s()) {
+ set_%s(other.%s());
}
)code",
field.name, field.name, field.name));
} else {
copiers.emplace_back(absl::StrFormat(R"code(
- if (other->has_%s()) {
- if (!CHECK_NOTNULL(add_%s())->FromFlatbuffer(other->%s())) {
+ if (other.has_%s()) {
+ if (!CHECK_NOTNULL(add_%s())->FromFlatbuffer(other.%s())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
@@ -473,11 +565,16 @@
// returning true on success.
// This is a deep copy, and will call FromFlatbuffer on any constituent
// objects.
- [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer &other) {
Clear();
%s
return true;
}
+ // Equivalent to FromFlatbuffer(const Flatbuffer&); this overload is provided
+ // to ease implementation of the aos::fbs::Vector internals.
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+ return FromFlatbuffer(*CHECK_NOTNULL(other));
+ }
)code",
absl::StrJoin(copiers, "\n"));
}
@@ -689,6 +786,7 @@
public:
// The underlying "raw" flatbuffer type for this type.
typedef %s Flatbuffer;
+ typedef flatbuffers::unique_ptr<Flatbuffer::NativeTableType> FlatbufferObjectType;
// Returns this object as a flatbuffer type. This reference may not be valid
// following mutations to the underlying flatbuffer, due to how memory may get
// may get moved around.
@@ -699,6 +797,7 @@
%s
%s
%s
+%s
private:
%s
%s
@@ -708,7 +807,7 @@
)code",
type_namespace, type_name, FlatbufferNameToCppName(fbs_type_name),
constants, MakeConstructor(type_name), type_name, accessors,
- MakeFullClearer(fields), MakeCopier(fields),
+ MakeFullClearer(fields), MakeCopier(fields), MakeObjectCopier(fields),
MakeMoveConstructor(type_name), members, MakeSubObjectList(fields));
GeneratedObject result;
diff --git a/aos/flatbuffers/static_flatbuffers_test.cc b/aos/flatbuffers/static_flatbuffers_test.cc
index fd95db3..52fa01e 100644
--- a/aos/flatbuffers/static_flatbuffers_test.cc
+++ b/aos/flatbuffers/static_flatbuffers_test.cc
@@ -10,6 +10,7 @@
#include "aos/flatbuffers.h"
#include "aos/flatbuffers/builder.h"
#include "aos/flatbuffers/interesting_schemas.h"
+#include "aos/flatbuffers/test_dir/include_reflection_static.h"
#include "aos/flatbuffers/test_dir/type_coverage_static.h"
#include "aos/flatbuffers/test_schema.h"
#include "aos/flatbuffers/test_static.h"
@@ -1042,4 +1043,55 @@
TestMemory(builder.buffer());
}
+// Uses a small example to manually verify that we can copy from the flatbuffer
+// object API.
+TEST_F(StaticFlatbuffersTest, ObjectApiCopy) {
+ aos::fbs::testing::TestTableT object_t;
+ object_t.scalar = 971;
+ object_t.vector_of_strings.push_back("971");
+ object_t.vector_of_structs.push_back({1, 2});
+ object_t.subtable = std::make_unique<SubTableT>();
+ aos::fbs::VectorAllocator allocator;
+ Builder<TestTableStatic> builder(&allocator);
+ ASSERT_TRUE(builder->FromFlatbuffer(object_t));
+ ASSERT_TRUE(builder.AsFlatbufferSpan().Verify());
+ // Note that vectors and strings get set to zero-length, but present, values.
+ EXPECT_EQ(
+ "{ \"scalar\": 971, \"vector_of_scalars\": [ ], \"string\": \"\", "
+ "\"vector_of_strings\": [ \"971\" ], \"subtable\": { \"foo\": 0, "
+ "\"baz\": 0.0 }, \"vector_aligned\": [ ], \"vector_of_structs\": [ { "
+ "\"x\": 1.0, \"y\": 2.0 } ], \"vector_of_tables\": [ ], "
+ "\"unspecified_length_vector\": [ ], \"unspecified_length_string\": "
+ "\"\", \"unspecified_length_vector_of_strings\": [ ] }",
+ aos::FlatbufferToJson(builder.AsFlatbufferSpan()));
+}
+
+// More completely covers our object API copying by comparing the flatbuffer
+// Pack() methods to our FromFlatbuffer() methods.
+TEST_F(StaticFlatbuffersTest, FlatbufferObjectTypeCoverage) {
+ VerifyJson<aos::testing::ConfigurationStatic>("{\n\n}");
+ std::string populated_config =
+ aos::util::ReadFileToStringOrDie(aos::testing::ArtifactPath(
+ "aos/flatbuffers/test_dir/type_coverage.json"));
+ Builder<aos::testing::ConfigurationStatic> json_builder =
+ aos::JsonToStaticFlatbuffer<aos::testing::ConfigurationStatic>(
+ populated_config);
+ aos::testing::ConfigurationT object_t;
+ json_builder->AsFlatbuffer().UnPackTo(&object_t);
+
+ Builder<aos::testing::ConfigurationStatic> from_object_static;
+ ASSERT_TRUE(from_object_static->FromFlatbuffer(object_t));
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.Finish(aos::testing::Configuration::Pack(fbb, &object_t));
+ aos::FlatbufferDetachedBuffer<aos::testing::Configuration> from_object_raw =
+ fbb.Release();
+ EXPECT_EQ(aos::FlatbufferToJson(from_object_raw, {.multi_line = true}),
+ aos::FlatbufferToJson(from_object_static, {.multi_line = true}));
+}
+
+// Tests that we can build code that uses the reflection types.
+TEST_F(StaticFlatbuffersTest, IncludeReflectionTypes) {
+ VerifyJson<::aos::testing::UseSchemaStatic>("{\n\n}");
+}
+
} // namespace aos::fbs::testing
diff --git a/aos/flatbuffers/static_vector.h b/aos/flatbuffers/static_vector.h
index 7349a8d..6133075 100644
--- a/aos/flatbuffers/static_vector.h
+++ b/aos/flatbuffers/static_vector.h
@@ -203,6 +203,11 @@
typename internal::InlineWrapper<T, kInline>::FlatbufferType;
using ConstFlatbufferType =
typename internal::InlineWrapper<T, kInline>::ConstFlatbufferType;
+ // FlatbufferObjectType corresponds to the type used by the flatbuffer
+ // "object" API (i.e. the FlatbufferT types).
+ // This type will be something unintelligble for inline types.
+ using FlatbufferObjectType =
+ typename internal::InlineWrapper<T, kInline>::FlatbufferObjectType;
// flatbuffers::Vector type that corresponds to this Vector.
typedef flatbuffers::Vector<FlatbufferType> Flatbuffer;
typedef const flatbuffers::Vector<ConstFlatbufferType> ConstFlatbuffer;
@@ -329,7 +334,70 @@
// we can allocate through reserve()).
// This is a deep copy, and will call FromFlatbuffer on any constituent
// objects.
- [[nodiscard]] bool FromFlatbuffer(ConstFlatbuffer *vector);
+ [[nodiscard]] bool FromFlatbuffer(ConstFlatbuffer *vector) {
+ return FromFlatbuffer(*CHECK_NOTNULL(vector));
+ }
+ [[nodiscard]] bool FromFlatbuffer(ConstFlatbuffer &vector);
+ // The remaining FromFlatbuffer() overloads are for when using the flatbuffer
+ // "object" API, which uses std::vector's for representing vectors.
+ [[nodiscard]] bool FromFlatbuffer(const std::vector<InlineType> &vector) {
+ static_assert(kInline);
+ return FromData(vector.data(), vector.size());
+ }
+ // Overload for vectors of bools, since the standard library may not use a
+ // full byte per vector element.
+ [[nodiscard]] bool FromFlatbuffer(const std::vector<bool> &vector) {
+ static_assert(kInline);
+ // We won't be able to do a clean memcpy because std::vector<bool> may be
+ // implemented using bit-packing.
+ return FromIterator(vector.cbegin(), vector.cend());
+ }
+ // Overload for non-inline types. Note that to avoid having this overload get
+ // resolved with inline types, we make FlatbufferObjectType != InlineType.
+ [[nodiscard]] bool FromFlatbuffer(
+ const std::vector<FlatbufferObjectType> &vector) {
+ static_assert(!kInline);
+ return FromNotInlineIterable(vector);
+ }
+
+ // Copies values from the provided data pointer into the vector, resizing the
+ // vector as needed to match. Returns false on failure (e.g., if the
+ // underlying allocator has insufficient space to perform the copy). Only
+ // works for inline data types.
+ [[nodiscard]] bool FromData(const InlineType *input_data, size_t input_size) {
+ static_assert(kInline);
+ if (!reserve(input_size)) {
+ return false;
+ }
+
+ // We will be overwriting the whole vector very shortly; there is no need to
+ // clear the buffer to zero.
+ resize_inline(input_size, SetZero::kNo);
+
+ memcpy(inline_data(), input_data, size() * sizeof(InlineType));
+ return true;
+ }
+
+ // Copies values from the provided iterators into the vector, resizing the
+ // vector as needed to match. Returns false on failure (e.g., if the
+ // underlying allocator has insufficient space to perform the copy). Only
+ // works for inline data types.
+ // Does not attempt any optimizations if the iterators meet the
+ // std::contiguous_iterator concept; instead, it simply copies each element
+ // out one-by-one.
+ template <typename Iterator>
+ [[nodiscard]] bool FromIterator(Iterator begin, Iterator end) {
+ static_assert(kInline);
+ resize(0);
+ for (Iterator it = begin; it != end; ++it) {
+ if (!reserve(size() + 1)) {
+ return false;
+ }
+ // Should never fail, due to the reserve() above.
+ CHECK(emplace_back(*it));
+ }
+ return true;
+ }
// Returns the element at the provided index. index must be less than size().
const T &at(size_t index) const {
@@ -569,29 +637,22 @@
}
// Implementation that handles copying from a flatbuffers::Vector of an inline
// data type.
- [[nodiscard]] bool FromInlineFlatbuffer(ConstFlatbuffer *vector) {
- if (!reserve(CHECK_NOTNULL(vector)->size())) {
- return false;
- }
-
- // We will be overwriting the whole vector very shortly; there is no need to
- // clear the buffer to zero.
- resize_inline(vector->size(), SetZero::kNo);
-
- memcpy(inline_data(), vector->Data(), size() * sizeof(InlineType));
- return true;
+ [[nodiscard]] bool FromInlineFlatbuffer(ConstFlatbuffer &vector) {
+ return FromData(reinterpret_cast<const InlineType *>(vector.Data()),
+ vector.size());
}
// Implementation that handles copying from a flatbuffers::Vector of a
// not-inline data type.
- [[nodiscard]] bool FromNotInlineFlatbuffer(const Flatbuffer *vector) {
- if (!reserve(vector->size())) {
+ template <typename Iterable>
+ [[nodiscard]] bool FromNotInlineIterable(const Iterable &vector) {
+ if (!reserve(vector.size())) {
return false;
}
// "Clear" the vector.
resize_not_inline(0);
- for (const typename T::Flatbuffer *entry : *vector) {
+ for (const auto &entry : vector) {
if (!CHECK_NOTNULL(emplace_back())->FromFlatbuffer(entry)) {
return false;
}
@@ -599,6 +660,10 @@
return true;
}
+ [[nodiscard]] bool FromNotInlineFlatbuffer(const Flatbuffer &vector) {
+ return FromNotInlineIterable(vector);
+ }
+
// In order to allow for easy partial template specialization, we use a
// non-member class to call FromInline/FromNotInlineFlatbuffer and
// resize_inline/resize_not_inline. There are not actually any great ways to
@@ -659,6 +724,7 @@
public:
typedef Vector<char, kStaticLength, true, 0, true> VectorType;
typedef flatbuffers::String Flatbuffer;
+ typedef std::string FlatbufferObjectType;
String(std::span<uint8_t> buffer, ResizeableObject *parent)
: VectorType(buffer, parent) {}
virtual ~String() {}
@@ -667,6 +733,10 @@
VectorType::resize_inline(string.size(), SetZero::kNo);
memcpy(VectorType::data(), string.data(), string.size());
}
+ using VectorType::FromFlatbuffer;
+ [[nodiscard]] bool FromFlatbuffer(const std::string &string) {
+ return VectorType::FromData(string.data(), string.size());
+ }
std::string_view string_view() const {
return std::string_view(VectorType::data(), VectorType::size());
}
@@ -690,12 +760,13 @@
typedef T ObjectType;
typedef flatbuffers::Offset<typename T::Flatbuffer> FlatbufferType;
typedef flatbuffers::Offset<typename T::Flatbuffer> ConstFlatbufferType;
+ typedef T::FlatbufferObjectType FlatbufferObjectType;
static_assert((T::kSize % T::kAlign) == 0);
static constexpr size_t kDataAlign = T::kAlign;
static constexpr size_t kDataSize = T::kSize;
template <typename StaticVector>
static bool FromFlatbuffer(
- StaticVector *to, const typename StaticVector::ConstFlatbuffer *from) {
+ StaticVector *to, const typename StaticVector::ConstFlatbuffer &from) {
return to->FromNotInlineFlatbuffer(from);
}
template <typename StaticVector>
@@ -712,11 +783,12 @@
typedef T ObjectType;
typedef T FlatbufferType;
typedef T ConstFlatbufferType;
+ typedef T *FlatbufferObjectType;
static constexpr size_t kDataAlign = alignof(T);
static constexpr size_t kDataSize = sizeof(T);
template <typename StaticVector>
static bool FromFlatbuffer(
- StaticVector *to, const typename StaticVector::ConstFlatbuffer *from) {
+ StaticVector *to, const typename StaticVector::ConstFlatbuffer &from) {
return to->FromInlineFlatbuffer(from);
}
template <typename StaticVector>
@@ -731,11 +803,12 @@
typedef uint8_t ObjectType;
typedef uint8_t FlatbufferType;
typedef uint8_t ConstFlatbufferType;
+ typedef uint8_t *FlatbufferObjectType;
static constexpr size_t kDataAlign = 1u;
static constexpr size_t kDataSize = 1u;
template <typename StaticVector>
static bool FromFlatbuffer(
- StaticVector *to, const typename StaticVector::ConstFlatbuffer *from) {
+ StaticVector *to, const typename StaticVector::ConstFlatbuffer &from) {
return to->FromInlineFlatbuffer(from);
}
template <typename StaticVector>
@@ -753,11 +826,12 @@
typedef T ObjectType;
typedef T *FlatbufferType;
typedef const T *ConstFlatbufferType;
+ typedef T *FlatbufferObjectType;
static constexpr size_t kDataAlign = alignof(T);
static constexpr size_t kDataSize = sizeof(T);
template <typename StaticVector>
static bool FromFlatbuffer(
- StaticVector *to, const typename StaticVector::ConstFlatbuffer *from) {
+ StaticVector *to, const typename StaticVector::ConstFlatbuffer &from) {
return to->FromInlineFlatbuffer(from);
}
template <typename StaticVector>
@@ -770,7 +844,7 @@
template <typename T, size_t kStaticLength, bool kInline, size_t kForceAlign,
bool kNullTerminate>
bool Vector<T, kStaticLength, kInline, kForceAlign,
- kNullTerminate>::FromFlatbuffer(ConstFlatbuffer *vector) {
+ kNullTerminate>::FromFlatbuffer(ConstFlatbuffer &vector) {
return internal::InlineWrapper<T, kInline>::FromFlatbuffer(this, vector);
}
diff --git a/aos/flatbuffers/test_dir/BUILD b/aos/flatbuffers/test_dir/BUILD
index 76f5fb4..a6275a5 100644
--- a/aos/flatbuffers/test_dir/BUILD
+++ b/aos/flatbuffers/test_dir/BUILD
@@ -1,6 +1,13 @@
load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
static_flatbuffer(
+ name = "include_reflection_fbs",
+ srcs = ["include_reflection.fbs"],
+ visibility = ["//visibility:public"],
+ deps = ["//aos/flatbuffers/reflection:reflection_fbs"],
+)
+
+static_flatbuffer(
name = "include_fbs",
srcs = ["include.fbs"],
visibility = ["//visibility:public"],
diff --git a/aos/flatbuffers/test_dir/include_reflection.fbs b/aos/flatbuffers/test_dir/include_reflection.fbs
new file mode 100644
index 0000000..7eda4f5
--- /dev/null
+++ b/aos/flatbuffers/test_dir/include_reflection.fbs
@@ -0,0 +1,9 @@
+include "reflection/reflection.fbs";
+
+namespace aos.testing;
+
+table UseSchema {
+ schema:reflection.Schema (id: 0);
+}
+
+root_type UseSchema;
diff --git a/aos/flatbuffers/test_dir/sample_test_static.h b/aos/flatbuffers/test_dir/sample_test_static.h
index a6362d7..57ac57a 100644
--- a/aos/flatbuffers/test_dir/sample_test_static.h
+++ b/aos/flatbuffers/test_dir/sample_test_static.h
@@ -14,6 +14,8 @@
public:
// The underlying "raw" flatbuffer type for this type.
typedef aos::fbs::testing::MinimallyAlignedTable Flatbuffer;
+ typedef flatbuffers::unique_ptr<Flatbuffer::NativeTableType>
+ FlatbufferObjectType;
// Returns this object as a flatbuffer type. This reference may not be valid
// following mutations to the underlying flatbuffer, due to how memory may get
// may get moved around.
@@ -135,15 +137,39 @@
// returning true on success.
// This is a deep copy, and will call FromFlatbuffer on any constituent
// objects.
- [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer &other) {
Clear();
- if (other->has_field()) {
- set_field(other->field());
+ if (other.has_field()) {
+ set_field(other.field());
}
return true;
}
+ // Equivalent to FromFlatbuffer(const Flatbuffer&); this overload is provided
+ // to ease implementation of the aos::fbs::Vector internals.
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+ return FromFlatbuffer(*CHECK_NOTNULL(other));
+ }
+
+ // Copies the contents of the provided flatbuffer into this flatbuffer,
+ // returning true on success.
+ // Because the Flatbuffer Object API does not provide any concept of an
+ // optionally populated scalar field, all scalar fields will be populated
+ // after a call to FromFlatbufferObject().
+ // This is a deep copy, and will call FromFlatbufferObject on
+ // any constituent objects.
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer::NativeTableType &other) {
+ Clear();
+
+ set_field(other.field);
+
+ return true;
+ }
+ [[nodiscard]] bool FromFlatbuffer(
+ const flatbuffers::unique_ptr<Flatbuffer::NativeTableType> &other) {
+ return FromFlatbuffer(*other);
+ }
private:
// We need to provide a MoveConstructor to allow this table to be
@@ -168,6 +194,8 @@
public:
// The underlying "raw" flatbuffer type for this type.
typedef aos::fbs::testing::SubTable Flatbuffer;
+ typedef flatbuffers::unique_ptr<Flatbuffer::NativeTableType>
+ FlatbufferObjectType;
// Returns this object as a flatbuffer type. This reference may not be valid
// following mutations to the underlying flatbuffer, due to how memory may get
// may get moved around.
@@ -314,19 +342,45 @@
// returning true on success.
// This is a deep copy, and will call FromFlatbuffer on any constituent
// objects.
- [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer &other) {
Clear();
- if (other->has_baz()) {
- set_baz(other->baz());
+ if (other.has_baz()) {
+ set_baz(other.baz());
}
- if (other->has_foo()) {
- set_foo(other->foo());
+ if (other.has_foo()) {
+ set_foo(other.foo());
}
return true;
}
+ // Equivalent to FromFlatbuffer(const Flatbuffer&); this overload is provided
+ // to ease implementation of the aos::fbs::Vector internals.
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+ return FromFlatbuffer(*CHECK_NOTNULL(other));
+ }
+
+ // Copies the contents of the provided flatbuffer into this flatbuffer,
+ // returning true on success.
+ // Because the Flatbuffer Object API does not provide any concept of an
+ // optionally populated scalar field, all scalar fields will be populated
+ // after a call to FromFlatbufferObject().
+ // This is a deep copy, and will call FromFlatbufferObject on
+ // any constituent objects.
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer::NativeTableType &other) {
+ Clear();
+
+ set_baz(other.baz);
+
+ set_foo(other.foo);
+
+ return true;
+ }
+ [[nodiscard]] bool FromFlatbuffer(
+ const flatbuffers::unique_ptr<Flatbuffer::NativeTableType> &other) {
+ return FromFlatbuffer(*other);
+ }
private:
// We need to provide a MoveConstructor to allow this table to be
@@ -354,6 +408,8 @@
public:
// The underlying "raw" flatbuffer type for this type.
typedef aos::fbs::testing::TestTable Flatbuffer;
+ typedef flatbuffers::unique_ptr<Flatbuffer::NativeTableType>
+ FlatbufferObjectType;
// Returns this object as a flatbuffer type. This reference may not be valid
// following mutations to the underlying flatbuffer, due to how memory may get
// may get moved around.
@@ -1060,109 +1116,108 @@
// returning true on success.
// This is a deep copy, and will call FromFlatbuffer on any constituent
// objects.
- [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer &other) {
Clear();
- if (other->has_included_table()) {
+ if (other.has_included_table()) {
if (!CHECK_NOTNULL(add_included_table())
- ->FromFlatbuffer(other->included_table())) {
+ ->FromFlatbuffer(other.included_table())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
}
}
- if (other->has_scalar()) {
- set_scalar(other->scalar());
+ if (other.has_scalar()) {
+ set_scalar(other.scalar());
}
- if (other->has_string()) {
- if (!CHECK_NOTNULL(add_string())->FromFlatbuffer(other->string())) {
+ if (other.has_string()) {
+ if (!CHECK_NOTNULL(add_string())->FromFlatbuffer(other.string())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
}
}
- if (other->has_substruct()) {
- set_substruct(*other->substruct());
+ if (other.has_substruct()) {
+ set_substruct(*other.substruct());
}
- if (other->has_subtable()) {
- if (!CHECK_NOTNULL(add_subtable())->FromFlatbuffer(other->subtable())) {
+ if (other.has_subtable()) {
+ if (!CHECK_NOTNULL(add_subtable())->FromFlatbuffer(other.subtable())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
}
}
- if (other->has_unspecified_length_string()) {
+ if (other.has_unspecified_length_string()) {
if (!CHECK_NOTNULL(add_unspecified_length_string())
- ->FromFlatbuffer(other->unspecified_length_string())) {
+ ->FromFlatbuffer(other.unspecified_length_string())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
}
}
- if (other->has_unspecified_length_vector()) {
+ if (other.has_unspecified_length_vector()) {
if (!CHECK_NOTNULL(add_unspecified_length_vector())
- ->FromFlatbuffer(other->unspecified_length_vector())) {
+ ->FromFlatbuffer(other.unspecified_length_vector())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
}
}
- if (other->has_unspecified_length_vector_of_strings()) {
+ if (other.has_unspecified_length_vector_of_strings()) {
if (!CHECK_NOTNULL(add_unspecified_length_vector_of_strings())
- ->FromFlatbuffer(
- other->unspecified_length_vector_of_strings())) {
+ ->FromFlatbuffer(other.unspecified_length_vector_of_strings())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
}
}
- if (other->has_vector_aligned()) {
+ if (other.has_vector_aligned()) {
if (!CHECK_NOTNULL(add_vector_aligned())
- ->FromFlatbuffer(other->vector_aligned())) {
+ ->FromFlatbuffer(other.vector_aligned())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
}
}
- if (other->has_vector_of_scalars()) {
+ if (other.has_vector_of_scalars()) {
if (!CHECK_NOTNULL(add_vector_of_scalars())
- ->FromFlatbuffer(other->vector_of_scalars())) {
+ ->FromFlatbuffer(other.vector_of_scalars())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
}
}
- if (other->has_vector_of_strings()) {
+ if (other.has_vector_of_strings()) {
if (!CHECK_NOTNULL(add_vector_of_strings())
- ->FromFlatbuffer(other->vector_of_strings())) {
+ ->FromFlatbuffer(other.vector_of_strings())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
}
}
- if (other->has_vector_of_structs()) {
+ if (other.has_vector_of_structs()) {
if (!CHECK_NOTNULL(add_vector_of_structs())
- ->FromFlatbuffer(other->vector_of_structs())) {
+ ->FromFlatbuffer(other.vector_of_structs())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
}
}
- if (other->has_vector_of_tables()) {
+ if (other.has_vector_of_tables()) {
if (!CHECK_NOTNULL(add_vector_of_tables())
- ->FromFlatbuffer(other->vector_of_tables())) {
+ ->FromFlatbuffer(other.vector_of_tables())) {
// Fail if we were unable to copy (e.g., if we tried to copy in a long
// vector and do not have the space for it).
return false;
@@ -1171,6 +1226,140 @@
return true;
}
+ // Equivalent to FromFlatbuffer(const Flatbuffer&); this overload is provided
+ // to ease implementation of the aos::fbs::Vector internals.
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer *other) {
+ return FromFlatbuffer(*CHECK_NOTNULL(other));
+ }
+
+ // Copies the contents of the provided flatbuffer into this flatbuffer,
+ // returning true on success.
+ // Because the Flatbuffer Object API does not provide any concept of an
+ // optionally populated scalar field, all scalar fields will be populated
+ // after a call to FromFlatbufferObject().
+ // This is a deep copy, and will call FromFlatbufferObject on
+ // any constituent objects.
+ [[nodiscard]] bool FromFlatbuffer(const Flatbuffer::NativeTableType &other) {
+ Clear();
+
+ if (other.included_table) {
+ if (!CHECK_NOTNULL(add_included_table())
+ ->FromFlatbuffer(*other.included_table)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+ }
+
+ set_scalar(other.scalar);
+
+ // Unconditionally copy strings/vectors, even if it will just end up
+ // being 0-length (this maintains consistency with the flatbuffer Pack()
+ // behavior).
+ if (!CHECK_NOTNULL(add_string())->FromFlatbuffer(other.string)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+
+ if (other.substruct) {
+ set_substruct(*other.substruct);
+ }
+
+ if (other.subtable) {
+ if (!CHECK_NOTNULL(add_subtable())->FromFlatbuffer(*other.subtable)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+ }
+
+ // Unconditionally copy strings/vectors, even if it will just end up
+ // being 0-length (this maintains consistency with the flatbuffer Pack()
+ // behavior).
+ if (!CHECK_NOTNULL(add_unspecified_length_string())
+ ->FromFlatbuffer(other.unspecified_length_string)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+
+ // Unconditionally copy strings/vectors, even if it will just end up
+ // being 0-length (this maintains consistency with the flatbuffer Pack()
+ // behavior).
+ if (!CHECK_NOTNULL(add_unspecified_length_vector())
+ ->FromFlatbuffer(other.unspecified_length_vector)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+
+ // Unconditionally copy strings/vectors, even if it will just end up
+ // being 0-length (this maintains consistency with the flatbuffer Pack()
+ // behavior).
+ if (!CHECK_NOTNULL(add_unspecified_length_vector_of_strings())
+ ->FromFlatbuffer(other.unspecified_length_vector_of_strings)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+
+ // Unconditionally copy strings/vectors, even if it will just end up
+ // being 0-length (this maintains consistency with the flatbuffer Pack()
+ // behavior).
+ if (!CHECK_NOTNULL(add_vector_aligned())
+ ->FromFlatbuffer(other.vector_aligned)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+
+ // Unconditionally copy strings/vectors, even if it will just end up
+ // being 0-length (this maintains consistency with the flatbuffer Pack()
+ // behavior).
+ if (!CHECK_NOTNULL(add_vector_of_scalars())
+ ->FromFlatbuffer(other.vector_of_scalars)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+
+ // Unconditionally copy strings/vectors, even if it will just end up
+ // being 0-length (this maintains consistency with the flatbuffer Pack()
+ // behavior).
+ if (!CHECK_NOTNULL(add_vector_of_strings())
+ ->FromFlatbuffer(other.vector_of_strings)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+
+ // Unconditionally copy strings/vectors, even if it will just end up
+ // being 0-length (this maintains consistency with the flatbuffer Pack()
+ // behavior).
+ if (!CHECK_NOTNULL(add_vector_of_structs())
+ ->FromFlatbuffer(other.vector_of_structs)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+
+ // Unconditionally copy strings/vectors, even if it will just end up
+ // being 0-length (this maintains consistency with the flatbuffer Pack()
+ // behavior).
+ if (!CHECK_NOTNULL(add_vector_of_tables())
+ ->FromFlatbuffer(other.vector_of_tables)) {
+ // Fail if we were unable to copy (e.g., if we tried to copy in a long
+ // vector and do not have the space for it).
+ return false;
+ }
+
+ return true;
+ }
+ [[nodiscard]] bool FromFlatbuffer(
+ const flatbuffers::unique_ptr<Flatbuffer::NativeTableType> &other) {
+ return FromFlatbuffer(*other);
+ }
private:
// We need to provide a MoveConstructor to allow this table to be
diff --git a/aos/json_to_flatbuffer.h b/aos/json_to_flatbuffer.h
index 6ef3544..deafaa7 100644
--- a/aos/json_to_flatbuffer.h
+++ b/aos/json_to_flatbuffer.h
@@ -75,6 +75,12 @@
Flatbuffer<T>::MiniReflectTypeTable(), json_options);
}
+template <typename T, typename Enable = T::Flatbuffer>
+inline ::std::string FlatbufferToJson(const fbs::Builder<T> &flatbuffer,
+ JsonOptions json_options = {}) {
+ return FlatbufferToJson(flatbuffer.AsFlatbufferSpan(), json_options);
+}
+
// Converts a flatbuffer::Table to JSON.
template <typename T>
typename std::enable_if<
diff --git a/aos/network/sctp_lib.cc b/aos/network/sctp_lib.cc
index 829ae67..cf50ad6 100644
--- a/aos/network/sctp_lib.cc
+++ b/aos/network/sctp_lib.cc
@@ -4,6 +4,7 @@
#include <linux/sctp.h>
#include <net/if.h>
#include <netdb.h>
+#include <netinet/ip.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/types.h>
@@ -26,6 +27,25 @@
DEFINE_bool(disable_ipv6, false, "disable ipv6");
DEFINE_int32(rmem, 0, "If nonzero, set rmem to this size.");
+// The Type of Service.
+// https://www.tucny.com/Home/dscp-tos
+//
+// We want to set the highest precedence (i.e. critical) with minimal delay. We
+// also want to be able to stuff the packets into bucket 0 for queue
+// disciplining. Experiments show that 176 works for this. Other values (e.g.
+// DSCP class EF) cannot be stuffed into bucket 0 (for unknown reasons).
+//
+// Note that the two least significant bits are reserved and should always set
+// to zero. Those two bits are the "Explicit Congestion Notification" bits. They
+// are controlled by the IP stack itself (and used by the router). We don't
+// control that via the TOS value we set here.
+DEFINE_int32(
+ sctp_tos, 176,
+ "The Type-Of-Service value to use. Defaults to a critical priority. "
+ "Always set values here whose two least significant bits are set to zero. "
+ "When using tcpdump, the `tos` field may show the least significant two "
+ "bits set to something other than zero.");
+
namespace aos::message_bridge {
namespace {
@@ -272,6 +292,13 @@
LOG(INFO) << "socket(" << Family(sockaddr_local)
<< ", SOCK_SEQPACKET, IPPROTOSCTP) = " << fd_;
{
+ // Set up Type-Of-Service.
+ //
+ // See comments for the --sctp_tos flag for more information.
+ int tos = IPTOS_DSCP(FLAGS_sctp_tos);
+ PCHECK(setsockopt(fd_, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)) == 0);
+ }
+ {
// Per https://tools.ietf.org/html/rfc6458
// Setting this to !0 allows event notifications to be interleaved
// with data if enabled. This typically only matters during congestion.
diff --git a/aos/starter/starterd_lib.cc b/aos/starter/starterd_lib.cc
index 95210c0..38d519d 100644
--- a/aos/starter/starterd_lib.cc
+++ b/aos/starter/starterd_lib.cc
@@ -19,6 +19,7 @@
DEFINE_uint32(queue_initialization_threads, 0,
"Number of threads to spin up to initialize the queue. 0 means "
"use the main thread.");
+DECLARE_bool(enable_ftrace);
namespace aos::starter {
@@ -214,6 +215,11 @@
if (info.ssi_signo == SIGCHLD) {
// SIGCHLD messages can be collapsed if multiple are received, so all
// applications must check their status.
+ if (FLAGS_enable_ftrace) {
+ ftrace_.FormatMessage("SIGCHLD");
+ ftrace_.TurnOffOrDie();
+ }
+
for (auto iter = applications_.begin(); iter != applications_.end();) {
if (iter->second.MaybeHandleSignal()) {
iter = applications_.erase(iter);
diff --git a/aos/starter/starterd_lib.h b/aos/starter/starterd_lib.h
index 3779c84..1ffc782 100644
--- a/aos/starter/starterd_lib.h
+++ b/aos/starter/starterd_lib.h
@@ -77,6 +77,8 @@
aos::PhasedLoopHandler *status_timer_;
aos::TimerHandler *cleanup_timer_;
+ aos::Ftrace ftrace_;
+
int status_count_ = 0;
const int max_status_count_;
diff --git a/aos/starter/subprocess.cc b/aos/starter/subprocess.cc
index 9606adc..d677922 100644
--- a/aos/starter/subprocess.cc
+++ b/aos/starter/subprocess.cc
@@ -266,6 +266,8 @@
if (application->has_memory_limit() && application->memory_limit() > 0) {
SetMemoryLimit(application->memory_limit());
}
+
+ set_stop_grace_period(std::chrono::nanoseconds(application->stop_time()));
}
void Application::DoStart() {
@@ -496,8 +498,7 @@
// Watchdog timer to SIGKILL application if it is still running 1 second
// after SIGINT
- stop_timer_->Schedule(event_loop_->monotonic_now() +
- std::chrono::seconds(1));
+ stop_timer_->Schedule(event_loop_->monotonic_now() + stop_grace_period_);
queue_restart_ = restart;
OnChange();
break;
diff --git a/aos/starter/subprocess.h b/aos/starter/subprocess.h
index ca7ef9d..eb36874 100644
--- a/aos/starter/subprocess.h
+++ b/aos/starter/subprocess.h
@@ -139,6 +139,14 @@
void set_capture_stderr(bool capture);
void set_run_as_sudo(bool value) { run_as_sudo_ = value; }
+ // Sets the time for a process to stop gracefully. If an application is asked
+ // to stop, but doesn't stop within the specified time limit, then it is
+ // forcefully killed. Defaults to 1 second unless overridden by the
+ // aos::Application instance in the constructor.
+ void set_stop_grace_period(std::chrono::nanoseconds stop_grace_period) {
+ stop_grace_period_ = stop_grace_period;
+ }
+
bool autostart() const { return autostart_; }
bool autorestart() const { return autorestart_; }
@@ -222,6 +230,7 @@
std::optional<uid_t> user_;
std::optional<gid_t> group_;
bool run_as_sudo_ = false;
+ std::chrono::nanoseconds stop_grace_period_ = std::chrono::seconds(1);
bool capture_stdout_ = false;
PipePair stdout_pipes_;
diff --git a/aos/starter/subprocess_reliable_test.cc b/aos/starter/subprocess_reliable_test.cc
index 0460bc3..701de11 100644
--- a/aos/starter/subprocess_reliable_test.cc
+++ b/aos/starter/subprocess_reliable_test.cc
@@ -3,6 +3,7 @@
#include <filesystem>
+#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "aos/events/shm_event_loop.h"
@@ -124,4 +125,55 @@
ASSERT_TRUE(std::filesystem::exists(shutdown_signal_file));
}
+// Validates that a process that is known to take a while to stop can shut down
+// gracefully without being killed.
+TEST(SubprocessTest, CanSlowlyStopGracefully) {
+ const std::string config_file =
+ ::aos::testing::ArtifactPath("aos/events/pingpong_config.json");
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(config_file);
+ aos::ShmEventLoop event_loop(&config.message());
+
+ // Use a file to signal that the subprocess has started up properly and that
+ // the exit handler has been installed. Otherwise we risk killing the process
+ // uncleanly before the signal handler got installed.
+ auto signal_dir = std::filesystem::path(aos::testing::TestTmpDir()) /
+ "slow_death_startup_file_signals";
+ ASSERT_TRUE(std::filesystem::create_directory(signal_dir));
+ auto startup_signal_file = signal_dir / "startup";
+
+ // Create an application that should never get killed automatically. It should
+ // have plenty of time to shut down on its own. In this case, we use 2 seconds
+ // to mean "plenty of time".
+ auto application = std::make_unique<Application>("/bin/bash", "/bin/bash",
+ &event_loop, [] {});
+ application->set_args(
+ {"-c",
+ absl::StrCat(
+ "trap 'echo got int; sleep 2; echo shutting down; exit 0' SIGINT; "
+ "while true; do sleep 0.1; touch ",
+ startup_signal_file.string(), "; done;")});
+ application->set_capture_stdout(true);
+ application->set_stop_grace_period(std::chrono::seconds(999));
+ application->AddOnChange([&] {
+ if (application->status() == aos::starter::State::STOPPED) {
+ event_loop.Exit();
+ }
+ });
+ application->Start();
+ event_loop
+ .AddTimer([&] {
+ if (std::filesystem::exists(startup_signal_file)) {
+ // Now that the subprocess has properly started up, let's kill it.
+ application->Stop();
+ }
+ })
+ ->Schedule(event_loop.monotonic_now(), std::chrono::milliseconds(100));
+ event_loop.Run();
+
+ EXPECT_EQ(application->exit_code(), 0);
+ EXPECT_THAT(application->GetStdout(), ::testing::HasSubstr("got int"));
+ EXPECT_THAT(application->GetStdout(), ::testing::HasSubstr("shutting down"));
+}
+
} // namespace aos::starter::testing
diff --git a/aos/time/time.cc b/aos/time/time.cc
index 6a25aa9..e75fe45 100644
--- a/aos/time/time.cc
+++ b/aos/time/time.cc
@@ -6,6 +6,7 @@
#include <cstring>
#include <ctime>
#include <iomanip>
+#include <sstream>
#ifdef __linux__
@@ -79,6 +80,18 @@
return stream;
}
+std::string ToString(const aos::monotonic_clock::time_point &now) {
+ std::ostringstream stream;
+ stream << now;
+ return stream.str();
+}
+
+std::string ToString(const aos::realtime_clock::time_point &now) {
+ std::ostringstream stream;
+ stream << now;
+ return stream.str();
+}
+
#ifdef __linux__
std::optional<monotonic_clock::time_point> monotonic_clock::FromString(
const std::string_view now) {
@@ -170,6 +183,16 @@
: std::chrono::duration_cast<std::chrono::seconds>(
now.time_since_epoch());
+ // We can run into some corner cases where the seconds value is large enough
+ // to cause the conversion to nanoseconds to overflow. That is undefined
+ // behaviour so we prevent it with this check here.
+ if (int64_t result;
+ __builtin_mul_overflow(seconds.count(), 1'000'000'000, &result)) {
+ stream << "(unrepresentable realtime " << now.time_since_epoch().count()
+ << ")";
+ return stream;
+ }
+
std::time_t seconds_t = seconds.count();
stream << std::put_time(localtime_r(&seconds_t, &tm), "%Y-%m-%d_%H-%M-%S.")
<< std::setfill('0') << std::setw(9)
diff --git a/aos/time/time.h b/aos/time/time.h
index 1a5cbd1..8462625 100644
--- a/aos/time/time.h
+++ b/aos/time/time.h
@@ -81,6 +81,9 @@
std::ostream &operator<<(std::ostream &stream,
const aos::realtime_clock::time_point &now);
+std::string ToString(const aos::monotonic_clock::time_point &now);
+std::string ToString(const aos::realtime_clock::time_point &now);
+
namespace time {
#ifdef __linux__
diff --git a/aos/time/time_test.cc b/aos/time/time_test.cc
index 63a145b..97116b0 100644
--- a/aos/time/time_test.cc
+++ b/aos/time/time_test.cc
@@ -208,8 +208,9 @@
std::stringstream s;
s << t;
- EXPECT_EQ(s.str(), "1677-09-21_00-12-43.145224192");
- EXPECT_EQ(realtime_clock::FromString(s.str()).value(), t);
+ // min_time happens to be unrepresentable because of rounding and signed
+ // integer overflow.
+ EXPECT_EQ(s.str(), "(unrepresentable realtime -9223372036854775808)");
}
{
@@ -224,4 +225,12 @@
}
}
+// Test that ToString works for monotonic and realtime time points.
+TEST(TimeTest, ToStringTimePoints) {
+ EXPECT_EQ(ToString(realtime_clock::epoch() + std::chrono::hours(5 * 24) +
+ std::chrono::seconds(11) + std::chrono::milliseconds(5)),
+ "1970-01-06_00-00-11.005000000");
+ EXPECT_EQ(ToString(monotonic_clock::min_time), "-9223372036.854775808sec");
+}
+
} // namespace aos::time::testing
diff --git a/aos/util/mcap_logger_test.cc b/aos/util/mcap_logger_test.cc
index c6febf9..3684e93 100644
--- a/aos/util/mcap_logger_test.cc
+++ b/aos/util/mcap_logger_test.cc
@@ -81,6 +81,20 @@
"values": {
"items": {
"properties": {
+ "attributes": {
+ "items": {
+ "properties": {
+ "key": {
+ "type": "string"
+ },
+ "value": {
+ "type": "string"
+ }
+ },
+ "type": "object"
+ },
+ "type": "array"
+ },
"documentation": {
"items": {
"type": "string"
diff --git a/aos/uuid.h b/aos/uuid.h
index 8dd93d4..a2cf8ae 100644
--- a/aos/uuid.h
+++ b/aos/uuid.h
@@ -8,6 +8,7 @@
#include "absl/types/span.h"
#include "flatbuffers/flatbuffers.h"
+#include "glog/logging.h"
namespace aos {
@@ -66,6 +67,11 @@
flatbuffers::Offset<flatbuffers::Vector<uint8_t>> PackVector(
flatbuffers::FlatBufferBuilder *fbb) const;
+ template <typename T>
+ void PackStaticVector(T *static_vector) const {
+ CHECK(static_vector->FromData(data_.data(), data_.size()));
+ }
+
// Returns a human-readable string representing this UUID.
//
// This is done without any memory allocation, which means it's returned in a
diff --git a/debian/BUILD.nghttp2.bazel b/debian/BUILD.nghttp2.bazel
new file mode 100644
index 0000000..cd055cd
--- /dev/null
+++ b/debian/BUILD.nghttp2.bazel
@@ -0,0 +1,71 @@
+load("@bazel_skylib//rules:write_file.bzl", "write_file")
+
+licenses(["notice"]) # MIT/X derivative license
+
+exports_files(["COPYING"])
+
+write_file(
+ name = "nghttp2ver.h",
+ out = "lib/includes/nghttp2/nghttp2ver.h",
+ content = """
+/*
+ * nghttp2 - HTTP/2 C Library
+ *
+ * Copyright (c) 2012, 2013 Tatsuhiro Tsujikawa
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#ifndef NGHTTP2VER_H
+#define NGHTTP2VER_H
+
+/**
+ * @macro
+ * Version number of the nghttp2 library release
+ */
+#define NGHTTP2_VERSION "1.58.0"
+
+/**
+ * @macro
+ * Numerical representation of the version number of the nghttp2 library
+ * release. This is a 24 bit number with 8 bits for major number, 8 bits
+ * for minor and 8 bits for patch. Version 1.2.3 becomes 0x010203.
+ */
+#define NGHTTP2_VERSION_NUM 0x013a00
+
+#endif /* NGHTTP2VER_H */
+""".splitlines(),
+)
+
+cc_library(
+ name = "nghttp2",
+ srcs = glob(["lib/*.c"]),
+ hdrs = glob(["lib/*.h"]) + [
+ "lib/includes/nghttp2/nghttp2.h",
+ ":nghttp2ver.h",
+ ],
+ defines = [
+ "HAVE_TIME_H",
+ "HAVE_ARPA_INET_H",
+ ],
+ includes = [
+ "lib/includes/",
+ ],
+ visibility = ["//visibility:public"],
+)
diff --git a/debian/curl.BUILD b/debian/curl.BUILD
index a4f6526..1292c5e 100644
--- a/debian/curl.BUILD
+++ b/debian/curl.BUILD
@@ -422,6 +422,7 @@
"-DCURL_DISABLE_NTLM", # turning it off in configure is not enough
"-DHAVE_LIBZ",
"-DHAVE_ZLIB_H",
+ "-DUSE_NGHTTP2",
"-Wno-string-plus-int",
"-Wno-cast-qual",
"-Wno-format-nonliteral",
@@ -466,6 +467,7 @@
# Use the same version of zlib that gRPC does.
"@zlib",
":define-ca-bundle-location",
+ "@com_github_nghttp2_nghttp2//:nghttp2",
] + select({
":windows": [],
"//conditions:default": [
diff --git a/documentation/aos/docs/flatbuffers.md b/documentation/aos/docs/flatbuffers.md
index 132348d..9c82ee3 100644
--- a/documentation/aos/docs/flatbuffers.md
+++ b/documentation/aos/docs/flatbuffers.md
@@ -157,13 +157,30 @@
type using the regular generated flatbuffer API and a `FromFlatbuffer()` method
which attempts to copy the specified flatbuffer into the current object.
+The `FromFlatbuffer()` method works on both the "raw" flatbuffer type, as well
+as on the [Flatbuffer Object
+API](https://flatbuffers.dev/flatbuffers_guide_use_cpp.html) (i.e. the
+`FlatbufferT` types). When copying
+flatbuffers from the object-based API, we apply the same semantics that that the
+`Pack()` method does in the raw flatbuffer type. Namely, all non-table fields
+will be set:
+
+ * Scalar fields are always populated, even if their value is equal to the
+ default.
+ * Vectors are set to zero-length vectors if there is no data in the vector.
+ * Strings are set to the empty string if there is no data in the string.
+
+These limitations are a consequence of how flatbuffers are represented in the
+object API, and is not an issue when copying from regular flatbuffer types.
+For copying from raw flatbuffer objects (which is what most existing code
+uses), these caveats do not apply, and there is no loss of information.
+
### Sample Usage
The below example constructs a table of the above example `TestTable`:
```cpp
-aos::FixedAllocator allocator(TestTableStatic::kUnalignedBufferSize);
-Builder<TestTableStatic> builder(&allocator);
+Builder<TestTableStatic> builder;
TestTableStatic *object = builder.get();
object->set_scalar(123);
{
@@ -436,10 +453,24 @@
space allocated for the vector; returns false on failure (e.g., if you are in
a fixed-size allocator that does not support increasing the size past a
certain point).
-* `bool FromFlatbuffer(const flatbuffers::Vector<>*)`: Attempts to copy an
+* `bool FromFlatbuffer(const flatbuffers::Vector<>&)`: Attempts to copy an
existing vector into this `Vector`. This may attempt to call `reserve()`
if the new vector is longer than `capacity()`. If the copy fails for
any reason, returns `false`.
+* `bool FromFlatbuffer(const std::vector<>&)`: Attempts to copy an
+ existing vector into this `Vector`. This may attempt to call `reserve()`
+ if the new vector is longer than `capacity()`. If the copy fails for
+ any reason, returns `false`. This is called "`FromFlatbuffer`" because
+ the [Flatbuffer Object
+ API](https://flatbuffers.dev/flatbuffers_guide_use_cpp.html) uses
+ `std::vector<>` to represent vectors.
+* `bool FromData(const T*, size_t)`: Attempts to copy a contiguous set of data
+ from the provided pointer. Only applies to inline types. This may attempt to
+ call `reserve()`, and if the call fails, it returns `false`.
+* `bool FromIterator(It begin, It end)`: Attempts to copy data from [begin, end)
+ into the vector. Does not assume that the data is stored contiguously in
+ memory. Only applies to inline types. This may attempt to
+ call `reserve()`, and if the call fails, it returns `false`.
#### Managing Resizing of Vectors
@@ -510,12 +541,25 @@
# upgraded to static_flatbuffer rules.
static_flatbuffer(
name = "test_message_fbs",
- src = "test_message.fbs",
+ srcs = ["test_message.fbs"],
)
```
+Then you must update the `#include` to use a `test_message_static.h` instead of
+the standard `test_message_generated.h` (the `_static.h` will include the
+`_generated.h` itself). Any C++ code can then be updated, noting that any
+AOS Senders that need to use the new API need to have their definitions updated
+to use the `TestMessageStatic` and must call `MakeStaticBuilder` instead of
+`MakeBuilder`. There is currently no support for using static flatbuffers with
+the AOS Fetcher or Watcher interfaces, as these only present immutable objects
+and so do not benefit from the API additions (and attempting to convert them to
+the new API would require additional overhead associated with copying the
+serialized flatbuffer into the correct format).
+
Before:
```cpp
+#include "aos/events/test_message_generated.h"
+...
aos::Sender<TestMessage> sender = loop1->MakeSender<TestMessage>("/test");
loop->OnRun([&]() {
@@ -528,6 +572,8 @@
After:
```cpp
+#include "aos/events/test_message_static.h"
+...
aos::Sender<TestMessageStatic> sender =
loop1->MakeSender<TestMessageStatic>("/test");
diff --git a/frc971/can_logger/can_logger.cc b/frc971/can_logger/can_logger.cc
index d7c2df7..cfc3dd8 100644
--- a/frc971/can_logger/can_logger.cc
+++ b/frc971/can_logger/can_logger.cc
@@ -5,10 +5,11 @@
CanLogger::CanLogger(aos::ShmEventLoop *event_loop,
std::string_view channel_name,
std::string_view interface_name)
- : fd_(socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW)),
- frames_sender_(event_loop->MakeSender<CanFrame>(channel_name)) {
+ : shm_event_loop_(event_loop),
+ fd_(socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW)),
+ frames_sender_(shm_event_loop_->MakeSender<CanFrame>(channel_name)) {
// TOOD(max): Figure out a proper priority
- event_loop->SetRuntimeRealtimePriority(10);
+ shm_event_loop_->SetRuntimeRealtimePriority(10);
struct ifreq ifr;
strcpy(ifr.ifr_name, interface_name.data());
PCHECK(ioctl(fd_.get(), SIOCGIFINDEX, &ifr) == 0)
@@ -34,7 +35,7 @@
CHECK_EQ(opt_size, sizeof(recieve_buffer_size));
VLOG(0) << "CAN recieve bufffer is " << recieve_buffer_size << " bytes large";
- event_loop->epoll()->OnReadable(fd_.get(), [this]() { Poll(); });
+ shm_event_loop_->epoll()->OnReadable(fd_.get(), [this]() { Poll(); });
}
void CanLogger::Poll() {
diff --git a/frc971/can_logger/can_logger.h b/frc971/can_logger/can_logger.h
index a144265..6bad877 100644
--- a/frc971/can_logger/can_logger.h
+++ b/frc971/can_logger/can_logger.h
@@ -33,6 +33,8 @@
CanLogger(const CanLogger &) = delete;
CanLogger &operator=(const CanLogger &) = delete;
+ ~CanLogger() { shm_event_loop_->epoll()->DeleteFd(fd_.get()); }
+
private:
void Poll();
@@ -40,6 +42,7 @@
// Returns true if successful and false if the recieve buffer is empty.
bool ReadFrame();
+ aos::ShmEventLoop *shm_event_loop_;
aos::ScopedFD fd_;
aos::Sender<CanFrame> frames_sender_;
};
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index afbce9b..c00ce90 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -42,7 +42,10 @@
static_flatbuffer(
name = "drivetrain_status_fbs",
srcs = ["drivetrain_status.fbs"],
- deps = ["//frc971/control_loops:control_loops_fbs"],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:encoder_fault_status_fbs",
+ ],
)
static_flatbuffer(
@@ -62,7 +65,10 @@
name = "drivetrain_status_ts_fbs",
srcs = ["drivetrain_status.fbs"],
target_compatible_with = ["@platforms//os:linux"],
- deps = ["//frc971/control_loops:control_loops_ts_fbs"],
+ deps = [
+ "//frc971/control_loops:control_loops_ts_fbs",
+ "//frc971/control_loops:encoder_fault_status_ts_fbs",
+ ],
)
flatbuffer_ts_library(
@@ -128,7 +134,10 @@
static_flatbuffer(
name = "drivetrain_status_float_fbs",
srcs = ["drivetrain_status_float.fbs"],
- deps = ["//frc971/control_loops:control_loops_fbs"],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:encoder_fault_status_fbs",
+ ],
)
aos_config(
@@ -136,6 +145,7 @@
src = "drivetrain_simulation_channels.json",
flatbuffers = [
":drivetrain_status_fbs",
+ "//frc971/control_loops:encoder_fault_status_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -162,6 +172,7 @@
":drivetrain_output_fbs",
":drivetrain_status_fbs",
":drivetrain_position_fbs",
+ ":drivetrain_can_position_fbs",
":localizer_fbs",
"//frc971/queues:gyro_fbs",
"//frc971/queues:gyro_uid_fbs",
@@ -789,6 +800,34 @@
],
)
+cc_library(
+ name = "drivetrain_encoder_fault_detector",
+ srcs = ["drivetrain_encoder_fault_detector.cc"],
+ hdrs = ["drivetrain_encoder_fault_detector.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:event_loop",
+ "//frc971/control_loops:encoder_fault_detector",
+ "//frc971/control_loops:encoder_fault_status_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ ],
+)
+
+cc_test(
+ name = "drivetrain_encoder_fault_detector_test",
+ srcs = ["drivetrain_encoder_fault_detector_test.cc"],
+ data = [":simulation_config"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":drivetrain_encoder_fault_detector",
+ "//aos:json_to_flatbuffer",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ ],
+)
+
cc_test(
name = "camera_test",
srcs = ["camera_test.cc"],
diff --git a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
index 29c0b85..539b18e 100644
--- a/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_can_position.fbs
@@ -14,6 +14,11 @@
// The ctre::phoenix::StatusCode of the measurement
// Should be OK = 0
status:int (id: 2);
+
+ // These values aren't used yet but are
+ // arrays that represent falcons on left/right sides of the drivetrain
+ left_falcons: [CANTalonFX] (id: 3);
+ right_falcons: [CANTalonFX] (id: 4);
}
root_type CANPosition;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
index a74d416..71c7628 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.json
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -43,6 +43,11 @@
},
{
"name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.CANPosition",
+ "frequency": 200
+ },
+ {
+ "name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.Status",
"frequency": 200,
"max_size": 2000
diff --git a/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.cc b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.cc
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.cc
diff --git a/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.h b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.h
new file mode 100644
index 0000000..7513033
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.h
@@ -0,0 +1,115 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_ENCODER_FAULT_DETECTOR_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_ENCODER_FAULT_DETECTOR_H_
+
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/encoder_fault_detector.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+// This class will be used to detect any encoder faults within the drivetrain
+// using values from the left/right encoders and motors
+
+template <uint8_t NumberofDrivetrainMotors>
+class DrivetrainEncoderFaultDetector {
+ public:
+ DrivetrainEncoderFaultDetector(aos::EventLoop *event_loop);
+
+ void UpdateDetectors();
+
+ void SendMessage();
+
+ flatbuffers::Offset<drivetrain::Faults> PopulateFaults(
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ private:
+ aos::EventLoop *event_loop_;
+ EncoderFaultDetector<NumberofDrivetrainMotors> left_encoder_fault_detector_;
+ EncoderFaultDetector<NumberofDrivetrainMotors> right_encoder_fault_detector_;
+ aos::Sender<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_sender_;
+ aos::Fetcher<control_loops::drivetrain::Position>
+ drivetrain_position_fetcher_;
+ aos::Fetcher<control_loops::drivetrain::CANPosition>
+ drivetrain_can_position_fetcher_;
+};
+
+template <uint8_t NumberofDrivetrainMotors>
+DrivetrainEncoderFaultDetector<NumberofDrivetrainMotors>::
+ DrivetrainEncoderFaultDetector(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ drivetrain_status_sender_(
+ event_loop->MakeSender<drivetrain::Status>("/drivetrain")),
+ drivetrain_position_fetcher_(
+ event_loop->MakeFetcher<drivetrain::Position>("/drivetrain")),
+ drivetrain_can_position_fetcher_(
+ event_loop->MakeFetcher<drivetrain::CANPosition>("/drivetrain")) {
+ event_loop
+ ->AddTimer([this]() {
+ UpdateDetectors();
+ SendMessage();
+ })
+ ->Schedule(event_loop->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+}
+
+template <uint8_t NumberofDrivetrainMotors>
+void DrivetrainEncoderFaultDetector<
+ NumberofDrivetrainMotors>::UpdateDetectors() {
+ drivetrain_position_fetcher_.Fetch();
+ drivetrain_can_position_fetcher_.Fetch();
+
+ if (drivetrain_position_fetcher_.get() &&
+ drivetrain_can_position_fetcher_.get()) {
+ left_encoder_fault_detector_.Iterate(
+ drivetrain_position_fetcher_->left_encoder(),
+ drivetrain_can_position_fetcher_->left_falcons(),
+ event_loop_->monotonic_now());
+ right_encoder_fault_detector_.Iterate(
+ drivetrain_position_fetcher_->right_encoder(),
+ drivetrain_can_position_fetcher_->right_falcons(),
+ event_loop_->monotonic_now());
+ }
+}
+
+template <uint8_t NumberofDrivetrainMotors>
+void DrivetrainEncoderFaultDetector<NumberofDrivetrainMotors>::SendMessage() {
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+ auto offset = PopulateFaults(builder.fbb());
+
+ drivetrain::Status::Builder drivetrain_status_builder =
+ builder.MakeBuilder<drivetrain::Status>();
+
+ drivetrain_status_builder.add_encoder_faults(offset);
+ builder.CheckOk(builder.Send(drivetrain_status_builder.Finish()));
+}
+
+template <uint8_t NumberofDrivetrainMotors>
+flatbuffers::Offset<drivetrain::Faults>
+DrivetrainEncoderFaultDetector<NumberofDrivetrainMotors>::PopulateFaults(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ flatbuffers::Offset<EncoderFaultStatus> left_encoder_fault_detector_offset =
+ left_encoder_fault_detector_.PopulateStatus(fbb);
+ flatbuffers::Offset<EncoderFaultStatus> right_encoder_fault_detector_offset =
+ right_encoder_fault_detector_.PopulateStatus(fbb);
+
+ drivetrain::Faults::Builder drivetrain_faults_builder(*fbb);
+
+ drivetrain_faults_builder.add_left_faulted(
+ left_encoder_fault_detector_offset);
+ drivetrain_faults_builder.add_right_faulted(
+ right_encoder_fault_detector_offset);
+
+ return drivetrain_faults_builder.Finish();
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif
\ No newline at end of file
diff --git a/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector_test.cc b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector_test.cc
new file mode 100644
index 0000000..adc8972
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector_test.cc
@@ -0,0 +1,416 @@
+#include "frc971/control_loops/drivetrain/drivetrain_encoder_fault_detector.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/json_to_flatbuffer.h"
+
+namespace frc971::control_loops::drivetrain::testing {
+
+class EncoderFaultDetectorTest : public ::testing::Test {
+ public:
+ EncoderFaultDetectorTest()
+ : config_(aos::configuration::ReadConfig(
+ "frc971/control_loops/drivetrain/simulation_config.json")),
+ event_loop_factory_(&config_.message()),
+ event_loop_(event_loop_factory_.MakeEventLoop(
+ "drivetrain_encoder_fault_detector", nullptr)),
+ drivetrain_status_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
+ drivetrain_position_sender_(
+ event_loop_->MakeSender<drivetrain::Position>("/drivetrain")),
+ drivetrain_can_position_sender_(
+ event_loop_->MakeSender<drivetrain::CANPosition>("/drivetrain")),
+ fault_detector_(event_loop_.get()) {}
+ void ResetEncoders() {
+ left_encoder_ = 0.0;
+ right_encoder_ = 0.0;
+ right_falcons_ = {0.0, 0.0};
+ left_falcons_ = {0.0, 0.0};
+ }
+ aos::FlatbufferDetachedBuffer<control_loops::drivetrain::Position>
+ ConstructPositionFlatBuffer(double left_encoder, double right_encoder) {
+ return aos::JsonToFlatbuffer<control_loops::drivetrain::Position>(
+ absl::StrFormat("{ \"left_encoder\": %f, \"right_encoder\": %f }",
+ left_encoder, right_encoder));
+ }
+
+ aos::FlatbufferDetachedBuffer<control_loops::drivetrain::CANPosition>
+ ConstructCANPositionFlatBuffer(const std::vector<double> &left_falcons,
+ const std::vector<double> &right_falcons) {
+ if (left_falcons.size() == right_falcons.size()) {
+ const size_t num_falcons = left_falcons.size();
+ std::string json = "{ \"left_falcons\":[";
+
+ for (size_t i = 0; i < num_falcons; ++i) {
+ json += absl::StrFormat("{ \"position\": %f }", left_falcons[i]);
+ if (i + 1 < num_falcons) {
+ json += ", ";
+ }
+ }
+
+ json += "], \"right_falcons\":[";
+
+ for (size_t i = 0; i < num_falcons; ++i) {
+ json += absl::StrFormat("{ \"position\": %f }", right_falcons[i]);
+ if (i + 1 < num_falcons) {
+ json += ", ";
+ }
+ }
+
+ json += "]}";
+ return aos::JsonToFlatbuffer<control_loops::drivetrain::CANPosition>(
+ json);
+ }
+ LOG(FATAL) << "You must provide two falcon arrays of equal length";
+ return aos::JsonToFlatbuffer<control_loops::drivetrain::CANPosition>("");
+ }
+
+ void SendPositionMessages() {
+ drivetrain_position_sender_.CheckOk(drivetrain_position_sender_.Send(
+ ConstructPositionFlatBuffer(left_encoder_, right_encoder_)));
+ drivetrain_can_position_sender_.CheckOk(
+ drivetrain_can_position_sender_.Send(
+ ConstructCANPositionFlatBuffer(left_falcons_, right_falcons_)));
+ }
+
+ protected:
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+ std::unique_ptr<aos::EventLoop> event_loop_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+ aos::Sender<control_loops::drivetrain::Position> drivetrain_position_sender_;
+ aos::Sender<control_loops::drivetrain::CANPosition>
+ drivetrain_can_position_sender_;
+
+ DrivetrainEncoderFaultDetector<2> fault_detector_;
+ double left_encoder_ = 0.0;
+ double right_encoder_ = 0.0;
+ std::vector<double> right_falcons_;
+ std::vector<double> left_falcons_;
+};
+
+// Test simulates if drivetrain encoders are idle
+TEST_F(EncoderFaultDetectorTest, Idle) {
+ ResetEncoders();
+
+ event_loop_->AddTimer([this]() { SendPositionMessages(); })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(1));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates if drivetrain encoders are increasing
+TEST_F(EncoderFaultDetectorTest, Increasing) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ left_encoder_ += 0.1;
+ right_encoder_ += 0.1;
+ for (double &falcon : left_falcons_) {
+ falcon += 0.1;
+ }
+ for (double &falcon : right_falcons_) {
+ falcon += 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates if drivetrain encoders are decreasing
+TEST_F(EncoderFaultDetectorTest, Decreasing) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ left_encoder_ -= 0.1;
+ right_encoder_ -= 0.1;
+ for (double &falcon : left_falcons_) {
+ falcon -= 0.1;
+ }
+ for (double &falcon : right_falcons_) {
+ falcon -= 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates if only the right drivetrain encoders are increasing
+TEST_F(EncoderFaultDetectorTest, OnlyIncreaseRightSide) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ right_encoder_ += 0.1;
+ for (double &falcon : right_falcons_) {
+ falcon += 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates if only the left drivetrain encoders are increasing
+TEST_F(EncoderFaultDetectorTest, OnlyIncreaseLeftSide) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ left_encoder_ += 0.1;
+ for (double &falcon : left_falcons_) {
+ falcon += 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates if only the right drivetrain encoders are decreasing
+TEST_F(EncoderFaultDetectorTest, OnlyDecreaseRightSide) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ right_encoder_ -= 0.1;
+ for (double &falcon : right_falcons_) {
+ falcon -= 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+// Test simulates if only the left drivetrain encoders are decreasing
+TEST_F(EncoderFaultDetectorTest, OnlyDecreaseLeftSide) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ left_encoder_ -= 0.1;
+ for (double &falcon : left_falcons_) {
+ falcon -= 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates that if there is no data for one second that their will be no
+// faults
+TEST_F(EncoderFaultDetectorTest, NoDataForOneSecond) {
+ ResetEncoders();
+
+ SendPositionMessages();
+
+ event_loop_factory_.RunFor(std::chrono::seconds(1));
+
+ left_encoder_ = 1.0;
+ right_encoder_ = 2.0;
+ for (double &falcon : left_falcons_) {
+ falcon = 3.0;
+ }
+ for (double &falcon : right_falcons_) {
+ falcon = 4.0;
+ }
+
+ SendPositionMessages();
+
+ event_loop_factory_.RunFor(std::chrono::seconds(1));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates that only the left encoder is increasing
+TEST_F(EncoderFaultDetectorTest, LeftEncoderFaulted) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ left_encoder_ += 0.1;
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ true,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+// Test simulates that only the right encoder is increasing
+TEST_F(EncoderFaultDetectorTest, RightEncoderFaulted) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ right_encoder_ += 0.1;
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ true,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates that only the left falcons are increasing
+TEST_F(EncoderFaultDetectorTest, LeftFalconsFaulted) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ for (double &falcon : left_falcons_) {
+ falcon += 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ true,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+// Test simulates that only the right falcons are increasing
+TEST_F(EncoderFaultDetectorTest, RightFalconsFaulted) {
+ ResetEncoders();
+
+ event_loop_
+ ->AddTimer([this]() {
+ SendPositionMessages();
+ for (double &falcon : right_falcons_) {
+ falcon += 0.1;
+ }
+ })
+ ->Schedule(event_loop_->monotonic_now(),
+ std::chrono::duration_cast<aos::monotonic_clock::duration>(
+ std::chrono::milliseconds(5)));
+ event_loop_factory_.RunFor(std::chrono::seconds(5));
+
+ CHECK(drivetrain_status_fetcher_.Fetch());
+
+ EXPECT_EQ(
+ true,
+ drivetrain_status_fetcher_->encoder_faults()->right_faulted()->faulted());
+ EXPECT_EQ(
+ false,
+ drivetrain_status_fetcher_->encoder_faults()->left_faulted()->faulted());
+}
+
+} // namespace frc971::control_loops::drivetrain::testing
\ No newline at end of file
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 8158e75..8e18915 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -1,4 +1,5 @@
include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/encoder_fault_status.fbs";
namespace frc971.control_loops.drivetrain;
@@ -210,6 +211,11 @@
accel_z_average:float (id: 8);
}
+table Faults {
+ right_faulted:EncoderFaultStatus (id: 0);
+ left_faulted:EncoderFaultStatus (id: 1);
+}
+
table Status {
// Estimated speed of the center of the robot in m/s (positive forwards).
robot_speed:double (id: 0);
@@ -271,6 +277,8 @@
// Total number of status send failures.
send_failures:uint64 (id: 28);
+
+ encoder_faults:Faults (id: 29);
}
root_type Status;
diff --git a/frc971/imu/BUILD b/frc971/imu/BUILD
index b90f844..3bccf20 100644
--- a/frc971/imu/BUILD
+++ b/frc971/imu/BUILD
@@ -61,3 +61,46 @@
uf2_from_elf(
name = "ADIS16505",
)
+
+cc_library(
+ name = "imu_calibrator",
+ srcs = ["imu_calibrator.cc"],
+ hdrs = [
+ "imu_calibrator.h",
+ "imu_calibrator-tmpl.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/time",
+ "//frc971/math:interpolate",
+ "@com_google_absl//absl/strings:str_format",
+ "@com_google_ceres_solver//:ceres",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_library(
+ name = "imu_calibrator_solver",
+ srcs = [
+ "imu_calibrator_solver.cc",
+ ],
+ hdrs = [
+ "imu_calibrator_solver.h",
+ ],
+ deps = [
+ ":imu_calibrator",
+ "@com_google_ceres_solver//:ceres",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_test(
+ name = "imu_calibrator_test",
+ srcs = ["imu_calibrator_test.cc"],
+ shard_count = 3,
+ deps = [
+ ":imu_calibrator",
+ ":imu_calibrator_solver",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/frc971/imu/imu_calibrator-tmpl.h b/frc971/imu/imu_calibrator-tmpl.h
new file mode 100644
index 0000000..6743106
--- /dev/null
+++ b/frc971/imu/imu_calibrator-tmpl.h
@@ -0,0 +1,248 @@
+#include "frc971/imu/imu_calibrator.h"
+#include "frc971/math/interpolate.h"
+
+DECLARE_int32(imu_zeroing_buffer);
+
+namespace frc971::imu {
+
+inline constexpr double kGravityGs = 1.0;
+// rad / sec
+inline constexpr double kGyroMaxZeroingValue = 0.1;
+
+template <typename Scalar>
+void ImuCalibrator<Scalar>::InsertImu(size_t imu_index,
+ const RawImuReading &reading) {
+ CHECK_LT(imu_index, imu_readings_.size());
+ std::vector<ImuReading> &readings = imu_readings_[imu_index];
+ if (readings.size() > 0u) {
+ CHECK_LT(readings.back().capture_time_raw, reading.capture_time)
+ << ": Readings must be inserted in time order per IMU.";
+ }
+ // Execute the stationary logic. We identify if this reading is plausibly
+ // stationary, then if it is not stationary, we go back in time to any
+ // potentially relevant readings and mark them as not stationary. Finally, we
+ // go through and as values exit the FLAGS_imu_zeroing_buffer window we do any
+ // processing that we can given that we know it must be stationary.
+ const bool plausibly_stationary =
+ reading.gyro.squaredNorm() < kGyroMaxZeroingValue * kGyroMaxZeroingValue;
+ bool stationary = plausibly_stationary;
+ int earliest_affected_index = readings.size() - FLAGS_imu_zeroing_buffer;
+ for (size_t index = std::max(0, earliest_affected_index);
+ index < readings.size(); ++index) {
+ if (!plausibly_stationary) {
+ readings[index].stationary = false;
+ }
+ if (!readings[index].plausibly_stationary) {
+ stationary = false;
+ }
+ }
+
+ // Since we don't have data from before the start, assume that we may have
+ // been moving.
+ if (earliest_affected_index < 0) {
+ stationary = false;
+ }
+
+ if (earliest_affected_index >= 0) {
+ ImuReading &earliest_reading = readings[earliest_affected_index];
+ // The stationary flag for this reading can no longer change, so we can
+ // start to do things based on it.
+ earliest_reading.stationary_is_locked = true;
+ if (earliest_reading.stationary) {
+ earliest_reading.parameter_residuals.gravity =
+ earliest_reading.accel.norm() - kGravityGs;
+ earliest_reading.parameter_residuals.gyro_zero = earliest_reading.gyro;
+ LOG(INFO) << earliest_reading.gyro.transpose();
+ }
+ }
+
+ const ImuConfig<Scalar> &config = imu_configs_[imu_index];
+ Scalar capture_time_adjusted =
+ static_cast<Scalar>(aos::time::DurationInSeconds(
+ reading.capture_time.time_since_epoch())) -
+ (config.parameters.has_value() ? config.parameters->time_offset
+ : static_cast<Scalar>(0.0));
+
+ imu_readings_[imu_index].emplace_back(
+ reading.capture_time, capture_time_adjusted,
+ reading.gyro - config.dynamic_params.gyro_zero,
+ reading.accel / config.dynamic_params.gravity,
+ DynamicImuParameters<Scalar>{static_cast<Scalar>(0.0),
+ Eigen::Matrix<Scalar, 3, 1>::Zero()},
+ plausibly_stationary, stationary, false, std::nullopt, std::nullopt);
+}
+
+template <typename Scalar>
+void ImuCalibrator<Scalar>::EvaluateRelativeResiduals() {
+ for (const auto &readings : imu_readings_) {
+ CHECK_LT(static_cast<size_t>(FLAGS_imu_zeroing_buffer * 2), readings.size())
+ << ": Insufficient readings to perform calibration.";
+ }
+ Scalar base_clock = imu_readings_[origin_index_][0].capture_time_adjusted;
+ // Current index corresponding to the base_clock time.
+ std::vector<size_t> reading_indices(imu_configs_.size(), 0);
+ // The for loops are set up so that we:
+ // 1. Iterate over every pair of readings from the origin/base IMU.
+ // 2. For each other IMU, we identify 0 or 1 readings that fall between those
+ // two readings of the origin IMU. We then calculate the residuals for
+ // that IMU relative to the origin IMU, linearly interpolating between
+ // the pair of readings from (1) (by doing a linear interpolation, we can
+ // get sub-cycle accuracy on time offsets).
+ for (;
+ reading_indices[origin_index_] < imu_readings_[origin_index_].size() - 1;
+ ++reading_indices[origin_index_]) {
+ const ImuReading &base_reading =
+ imu_readings_[origin_index_][reading_indices[origin_index_]];
+ const ImuReading &next_base_reading =
+ imu_readings_[origin_index_][reading_indices[origin_index_] + 1];
+ base_clock = base_reading.capture_time_adjusted;
+ const Scalar next_base_clock = next_base_reading.capture_time_adjusted;
+ for (size_t imu_index = 0; imu_index < imu_configs_.size(); ++imu_index) {
+ const ImuConfig<Scalar> &imu_config = imu_configs_[imu_index];
+ // We don't care about calculating the offsets from the origin IMU to
+ // itself...
+ if (imu_config.is_origin) {
+ continue;
+ }
+ auto &readings = imu_readings_[imu_index];
+ bool done_with_imu = false;
+ // This will put the index for the current IMU just past the base_clock
+ // timepoint, allowing us to interpolate between
+ // reading_indices[origin_index_] and reading_indices[origin_index_] + 1.
+ while (readings[reading_indices[imu_index]].capture_time_adjusted <
+ base_clock) {
+ if (reading_indices[imu_index] == imu_readings_[imu_index].size() - 1) {
+ done_with_imu = true;
+ break;
+ }
+ ++reading_indices[imu_index];
+ }
+ // If we've run out of readings on this imu, stop doing anything.
+ if (done_with_imu) {
+ continue;
+ }
+ ImuReading &reading = readings[reading_indices[imu_index]];
+ const Scalar reading_time = reading.capture_time_adjusted;
+ if (reading_time >= next_base_clock) {
+ // There is a gap in readings for this imu; we can't meaningfully
+ // populate the residuals.
+ continue;
+ }
+ // Sanity check the above logic.
+ CHECK_LE(base_clock, reading_time);
+ CHECK_LT(reading_time, next_base_clock);
+ CHECK(imu_config.parameters.has_value());
+ reading.gyro_residual =
+ imu_config.parameters.value().rotation * reading.gyro -
+ frc971::math::Interpolate<Eigen::Matrix<Scalar, 3, 1>, Scalar>(
+ base_clock, next_base_clock, base_reading.gyro,
+ next_base_reading.gyro, reading_time);
+ if (!reading.stationary_is_locked || !reading.stationary) {
+ continue;
+ }
+ // In order to calculate the accelerometer residual, we are assuming that
+ // the two IMUs "should" produce identical accelerations. This is only
+ // true when not rotating. Future changes may account for coriolis
+ // effects.
+ reading.accel_residual =
+ imu_config.parameters.value().rotation * reading.accel -
+ frc971::math::Interpolate(base_clock, next_base_clock,
+ base_reading.accel, next_base_reading.accel,
+ reading_time);
+ }
+ }
+}
+
+// Helpers to accommodate serializing residuals into the ceres buffer. These
+// helpers all return a buffer that points to the next value to be populated.
+namespace internal {
+template <typename Scalar>
+std::span<Scalar> SerializeScalar(Scalar value, std::span<Scalar> out) {
+ DCHECK(!out.empty());
+ out[0] = value;
+ return out.subspan(1);
+}
+template <typename Scalar, int kSize>
+std::span<Scalar> SerializeVector(const Eigen::Matrix<Scalar, kSize, 1> &value,
+ std::span<Scalar> out) {
+ DCHECK_LE(static_cast<size_t>(value.size()), out.size());
+ for (int index = 0; index < kSize; ++index) {
+ out[index] = value(index);
+ }
+ return out.subspan(kSize);
+}
+template <typename Scalar>
+std::span<Scalar> SerializeParams(const DynamicImuParameters<Scalar> ¶ms,
+ std::span<Scalar> out) {
+ return SerializeVector(params.gyro_zero,
+ SerializeScalar(params.gravity, out));
+}
+inline constexpr int kResidualsPerReading = 10u;
+} // namespace internal
+
+template <typename Scalar>
+void ImuCalibrator<Scalar>::CalculateResiduals(std::span<Scalar> residuals) {
+ EvaluateRelativeResiduals();
+ for (size_t imu_index = 0; imu_index < imu_configs_.size(); ++imu_index) {
+ const auto &readings = imu_readings_[imu_index];
+ double valid_gyro_reading_count = 0;
+ double valid_accel_reading_count = 0;
+ for (size_t reading_index = 0; reading_index < readings.size();
+ ++reading_index) {
+ const auto &reading = readings[reading_index];
+ if (reading.gyro_residual.has_value()) {
+ ++valid_gyro_reading_count;
+ }
+ if (reading.accel_residual.has_value()) {
+ ++valid_accel_reading_count;
+ }
+ }
+ if (!imu_configs_[imu_index].is_origin) {
+ CHECK_LT(0, valid_gyro_reading_count);
+ CHECK_LT(0, valid_accel_reading_count);
+ } else {
+ valid_gyro_reading_count = readings.size();
+ valid_accel_reading_count = readings.size();
+ }
+ // Adjust the residuals of the readings to ensure that the solver doesn't
+ // cheat by just making it so that the time-offsets are completely
+ // misaligned and we can say that all the residuals are "zero".
+ const Scalar gyro_reading_scalar =
+ static_cast<Scalar>(readings.size() / valid_gyro_reading_count);
+ const Scalar accel_reading_scalar =
+ static_cast<Scalar>(readings.size() / valid_accel_reading_count);
+ for (size_t reading_index = 0; reading_index < readings.size();
+ ++reading_index) {
+ const auto &reading = readings[reading_index];
+ const Scalar *const start_residual = residuals.data();
+ // 4 residuals (gravity scalar; gyro zeroes)
+ residuals =
+ internal::SerializeParams(reading.parameter_residuals, residuals);
+ const Eigen::Matrix<Scalar, 3, 1> gyro_residual =
+ reading.gyro_residual.value_or(Eigen::Matrix<Scalar, 3, 1>::Zero()) *
+ gyro_reading_scalar;
+ // 3 residuals
+ residuals = internal::SerializeVector(gyro_residual, residuals);
+ const Eigen::Matrix<Scalar, 3, 1> accel_residual =
+ reading.accel_residual.value_or(Eigen::Matrix<Scalar, 3, 1>::Zero()) *
+ accel_reading_scalar;
+ // 3 residuals
+ residuals = internal::SerializeVector(accel_residual, residuals);
+ CHECK_EQ(internal::kResidualsPerReading,
+ residuals.data() - start_residual)
+ << ": Need to update kResidualsPerReading.";
+ }
+ }
+}
+
+template <typename Scalar>
+size_t ImuCalibrator<Scalar>::CalculateNumResiduals(
+ const std::vector<size_t> &num_readings) {
+ size_t num_residuals = 0;
+ for (const size_t count : num_readings) {
+ num_residuals += internal::kResidualsPerReading * count;
+ }
+ return num_residuals;
+}
+
+} // namespace frc971::imu
diff --git a/frc971/imu/imu_calibrator.cc b/frc971/imu/imu_calibrator.cc
new file mode 100644
index 0000000..03fa377
--- /dev/null
+++ b/frc971/imu/imu_calibrator.cc
@@ -0,0 +1,6 @@
+#include "frc971/imu/imu_calibrator.h"
+
+DEFINE_int32(
+ imu_zeroing_buffer, 100,
+ "We will only consider readings stationary for purposes if calibration if "
+ "this many readings to either side appear to be stationary.");
diff --git a/frc971/imu/imu_calibrator.h b/frc971/imu/imu_calibrator.h
new file mode 100644
index 0000000..be4c4d4
--- /dev/null
+++ b/frc971/imu/imu_calibrator.h
@@ -0,0 +1,331 @@
+#ifndef FRC971_IMU_IMU_CALIBRATOR_H_
+#define FRC971_IMU_IMU_CALIBRATOR_H_
+
+#include <optional>
+#include <span>
+#include <tuple>
+#include <vector>
+
+#include "absl/strings/str_format.h"
+#include "absl/strings/str_join.h"
+#include "ceres/ceres.h"
+#include "glog/logging.h"
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "aos/time/time.h"
+
+namespace frc971::imu {
+
+// Contains a reading that comes directly from an IMU.
+// These should not be zeroed or corrected for yet.
+struct RawImuReading {
+ aos::monotonic_clock::time_point capture_time;
+ // gyro readings are in radians / sec; accelerometer readings are in g's.
+ const Eigen::Vector3d gyro;
+ const Eigen::Vector3d accel;
+};
+
+// Note on how we manage populating ceres parameters:
+// * We use dynamically-sized parameter lists for convenience.
+// * For every struct that we have that corresponds to problem parameters,
+// there is a PopulateParameters() method that takes a:
+// * ceres::Problem* that is used for setting parameter constraints.
+// * ceres::DynamicCostFunction* that is used to mark that we have added
+// a parameter block.
+// * A std::vector<double*> that will later be passed to AddResidualBlock.
+// We add any parameter blocks which we add to the problem.
+// * post_populate_methods is a list of std::function's that will get called
+// after everything is populated and added (this is necessary because
+// we cannot add constraints to the Problem until after everything
+// has been added).
+// * Additionally, there is a PopulateFromParameters() method which is used
+// in the ceres cost functor and:
+// * Takes a parameters double-pointer where parameters[X] is the
+// parameter block X.
+// * Returns a pair where the first value is a populated instance of the
+// struct and an updated parameters pointer which points to the next
+// set of pointers.
+// * All the Populate* methods must be called in the same order every
+// time so that they result in the raw pointers getting populated
+// consistently.
+
+// These are the parameters corresponding to things which will vary at every
+// power-on of the IMU.
+template <typename Scalar>
+struct DynamicImuParameters {
+ // A scalar to represent the current magnitude of gravity, in g's.
+ // This currently compensates both for any variations in local gravity as well
+ // as for some amount of variations in the IMU itself. In the future we may
+ // expand this to have a local gravity number that is global to all IMUs while
+ // separately calibrating per-axis information.
+ Scalar gravity;
+ // Current gyro zero, in radians / sec.
+ // These are the per-axis values that the gyro will read when it is sitting
+ // still. Technically these zeroes will drift over time; however, the
+ // time-windows that we expect to use for calibration are short enough that
+ // this should be a non-issue.
+ Eigen::Matrix<Scalar, 3, 1> gyro_zero;
+ void PopulateParameters(
+ ceres::Problem *problem, ceres::DynamicCostFunction *cost_function,
+ std::vector<double *> *parameters,
+ std::vector<std::function<void()>> *post_populate_methods) {
+ cost_function->AddParameterBlock(1);
+ parameters->push_back(&gravity);
+ cost_function->AddParameterBlock(3);
+ parameters->push_back(gyro_zero.data());
+ post_populate_methods->emplace_back([this, problem]() {
+ // Gravity shouldn't vary by much (these bounds are significantly larger
+ // than any real variations which we will experience).
+ problem->SetParameterLowerBound(&gravity, 0, 0.95);
+ problem->SetParameterUpperBound(&gravity, 0, 1.05);
+ for (int i = 0; i < 3; ++i) {
+ problem->SetParameterLowerBound(gyro_zero.data(), i, -0.05);
+ problem->SetParameterUpperBound(gyro_zero.data(), i, 0.05);
+ }
+ });
+ }
+ static std::tuple<DynamicImuParameters<Scalar>, const Scalar *const *>
+ PopulateFromParameters(const Scalar *const *parameters) {
+ const Scalar *const gravity = parameters[0];
+ ++parameters;
+ const Scalar *const gyro = parameters[0];
+ ++parameters;
+ return std::make_tuple(
+ DynamicImuParameters<Scalar>{
+ *gravity, Eigen::Matrix<Scalar, 3, 1>(gyro[0], gyro[1], gyro[2])},
+ parameters);
+ }
+ std::string ToString() const {
+ std::stringstream out;
+ out << "gravity: " << gravity << " gyro_zero: " << gyro_zero.transpose();
+ return out.str();
+ }
+};
+
+// These parameters correspond to IMU parameters which will not vary between
+// boots. Namely, the relative positions and time offsets of the IMU(s).
+template <typename Scalar>
+struct StaticImuParameters {
+ // If you have a point p_imu in this IMU's frame then (rotation *
+ // p_imu + position) will give you that point's position in the board frame
+ // (the "board" frame refers to the frame associated with the entire PCB,
+ // where the PCB itself contains multiple IMUs. The board frame will be
+ // attached to whichever IMU is being treated as the origin).
+ Eigen::Quaternion<Scalar> rotation;
+ // position is currently unused because it is only observeable when there
+ // are coriolis effects, and we currently only make use of accelerometer
+ // readings from when the IMU is sat still.
+ // As such, we currently assume that all position offsets are zero.
+ // Eigen::Matrix<Scalar, 3, 1> position;
+ // The "true" time at which the event occurred is equal to the capture time -
+ // time_offset. I.e., a more positive time offset implies that the there is a
+ // large delay between the imu readings being taken and us observing them.
+ Scalar time_offset;
+
+ void PopulateParameters(
+ ceres::EigenQuaternionParameterization *quaternion_local_parameterization,
+ ceres::Problem *problem, ceres::DynamicCostFunction *cost_function,
+ std::vector<double *> *parameters,
+ std::vector<std::function<void()>> *post_populate_methods) {
+ cost_function->AddParameterBlock(4);
+ parameters->push_back(rotation.coeffs().data());
+ cost_function->AddParameterBlock(1);
+ parameters->push_back(&time_offset);
+ post_populate_methods->emplace_back(
+ [this, problem, quaternion_local_parameterization]() {
+ problem->SetParameterization(rotation.coeffs().data(),
+ quaternion_local_parameterization);
+ problem->SetParameterLowerBound(&time_offset, 0, -0.03);
+ problem->SetParameterUpperBound(&time_offset, 0, 0.03);
+ });
+ }
+ static std::tuple<StaticImuParameters, const Scalar *const *>
+ PopulateFromParameters(const Scalar *const *parameters) {
+ const Scalar *const quat = parameters[0];
+ ++parameters;
+ const Scalar *const time = parameters[0];
+ ++parameters;
+ return std::make_tuple(
+ StaticImuParameters{
+ Eigen::Quaternion<Scalar>(quat[3], quat[0], quat[1], quat[2]),
+ *time},
+ parameters);
+ }
+ std::string ToString() const {
+ std::stringstream out;
+ out << "quat: " << rotation.coeffs().transpose()
+ << " time_offset: " << time_offset;
+ return out.str();
+ }
+};
+
+// Represents the calibration for a single IMU.
+template <typename Scalar>
+struct ImuConfig {
+ // Set to true if this IMU is to be considered the origin of the coordinate
+ // system. This will also mean that this IMU is treated as the source-of-truth
+ // for clock offsets.
+ bool is_origin;
+ // Will be nullopt if is_origin is true (the corresponding rotations and
+ // offsets will all be the identity/zero as appropriate).
+ std::optional<StaticImuParameters<Scalar>> parameters;
+
+ DynamicImuParameters<Scalar> dynamic_params{
+ .gravity = static_cast<Scalar>(1.0),
+ .gyro_zero = Eigen::Matrix<Scalar, 3, 1>::Zero()};
+ std::string ToString() const {
+ return absl::StrFormat(
+ "is_origin: %d params: %s dynamic: %s", is_origin,
+ parameters.has_value() ? parameters->ToString() : std::string("<null>"),
+ dynamic_params.ToString());
+ }
+};
+
+// Represents all of the configuration parameters for the entire system.
+template <typename Scalar>
+struct AllParameters {
+ // Each entry in the imus vector will be a single imu.
+ std::vector<ImuConfig<Scalar>> imus;
+ std::tuple<std::vector<double *>, std::vector<std::function<void()>>>
+ PopulateParameters(
+ ceres::EigenQuaternionParameterization *quaternion_local_parameterization,
+ ceres::Problem *problem, ceres::DynamicCostFunction *cost_function) {
+ std::vector<std::function<void()>> post_populate_methods;
+ std::vector<double *> parameters;
+ for (auto &imu : imus) {
+ if (imu.parameters.has_value()) {
+ imu.parameters.value().PopulateParameters(
+ quaternion_local_parameterization, problem, cost_function,
+ ¶meters, &post_populate_methods);
+ }
+ imu.dynamic_params.PopulateParameters(problem, cost_function, ¶meters,
+ &post_populate_methods);
+ }
+ return std::make_tuple(parameters, post_populate_methods);
+ }
+ static AllParameters PopulateFromParameters(
+ const std::vector<ImuConfig<double>> &nominal_configs,
+ const Scalar *const *parameters) {
+ std::vector<ImuConfig<Scalar>> result;
+ for (size_t imu_index = 0; imu_index < nominal_configs.size();
+ ++imu_index) {
+ ImuConfig<Scalar> config;
+ config.is_origin = nominal_configs[imu_index].is_origin;
+ if (!config.is_origin) {
+ std::tie(config.parameters, parameters) =
+ StaticImuParameters<Scalar>::PopulateFromParameters(parameters);
+ }
+ std::tie(config.dynamic_params, parameters) =
+ DynamicImuParameters<Scalar>::PopulateFromParameters(parameters);
+ result.emplace_back(std::move(config));
+ }
+ return {.imus = std::move(result)};
+ }
+ std::string ToString() const {
+ std::vector<std::string> imu_strings;
+ for (const auto &imu : imus) {
+ std::vector<std::string> dynamic_params;
+ imu_strings.push_back(absl::StrFormat("config: %s", imu.ToString()));
+ }
+ return absl::StrJoin(imu_strings, "\n");
+ }
+};
+
+// This class does the hard work to support calibrating multiple IMU's
+// orientations relative to one another. It is set up to readily be used with a
+// ceres solver (see imu_calibrator.*).
+//
+// The current theory of operation is to have some number of imus, one of which
+// we will consider to be fixed in position. We have a fixed set of data that we
+// feed into this class, which can be parameterized based on:
+// * The current zeroes for each IMU.
+// * The orientation of each non-fixed IMU.
+// * The time-offset of each non-fixed IMU.
+//
+// When run under ceres, ceres can then vary these parameters to solve for the
+// current calibrations of the IMUs.
+//
+// When solving, the main complexity is that some internal state has to be
+// tracked to try to determine when we should be calculating zeroes and when we
+// can calibrate out the magnitude of gravity. This is done by tracking when the
+// IMUs are "stationary". For a reading to be stationary, all values with
+// FLAGS_imu_zeroing_buffer readings of the reading must be "plausibly
+// stationary". Readings are plausibly stationary if they have sufficiently low
+// gyro values.
+//
+// TODO: Provide some utilities to plot the results of a calibration.
+template <typename Scalar>
+class ImuCalibrator {
+ public:
+ ImuCalibrator(const std::vector<ImuConfig<Scalar>> &imu_configs)
+ : imu_configs_(imu_configs), imu_readings_(imu_configs.size()) {
+ origin_index_ = -1;
+ for (size_t imu_index = 0; imu_index < imu_configs_.size(); ++imu_index) {
+ if (imu_configs_[imu_index].is_origin) {
+ CHECK_EQ(origin_index_, -1)
+ << ": Can't have more than one IMU specified as the origin.";
+ origin_index_ = imu_index;
+ }
+ }
+ CHECK_NE(origin_index_, -1)
+ << ": Must have at least one IMU specified as the origin.";
+ }
+
+ // These gyro readings will be "raw"---i.e., they still need to get
+ // transformed by nominal_transform.
+ // gyro readings are in radians / sec; accelerometer readings are in g's.
+ // Within a given imu, must be called in time order.
+ void InsertImu(size_t imu_index, const RawImuReading &reading);
+
+ // Populates all the residuals that we use for the cost in ceres.
+ void CalculateResiduals(std::span<Scalar> residuals);
+
+ // Returns the total number of residuals that this problem will have, given
+ // the number of readings for each IMU.
+ static size_t CalculateNumResiduals(const std::vector<size_t> &num_readings);
+
+ private:
+ // Represents an imu reading. The values in this are generally already
+ // adjusted for the provided parameters.
+ struct ImuReading {
+ // The "actual" provided capture time. Used for debugging.
+ aos::monotonic_clock::time_point capture_time_raw;
+ // The capture time, adjusted for this IMU's time offset.
+ Scalar capture_time_adjusted;
+ // gyro reading, adjusted for gyro zero but NOT rotation.
+ // In radians / sec.
+ Eigen::Matrix<Scalar, 3, 1> gyro;
+ // accelerometer reading, adjusted for gravity value but NOT rotation.
+ // In g's.
+ Eigen::Matrix<Scalar, 3, 1> accel;
+ // Residuals associated with the DynamicImuParameters for this imu.
+ DynamicImuParameters<Scalar> parameter_residuals;
+ // Set if this measurement *could* be part of a segment of time where the
+ // IMU is stationary.
+ bool plausibly_stationary;
+ // Set to true if all values with FLAGS_imu_zeroing_buffer of this reading
+ // are plausibly_stationary.
+ bool stationary;
+ // We set stationary_is_locked once we have enough readings to either side
+ // of this value to guarantee that it is stationary.
+ bool stationary_is_locked;
+ // Residuals that are used for calibrating the rotation values. These are
+ // nullopt if we can't calibrate for some reason (e.g., this is the origin
+ // IMU, or for the accelerometer residual, we don't populate it if we are
+ // not stationary).
+ std::optional<Eigen::Matrix<Scalar, 3, 1>> gyro_residual;
+ std::optional<Eigen::Matrix<Scalar, 3, 1>> accel_residual;
+ };
+ void EvaluateRelativeResiduals();
+
+ const std::vector<ImuConfig<Scalar>> imu_configs_;
+ // Index of the IMU which is the origin IMU.
+ int origin_index_;
+ std::vector<std::vector<ImuReading>> imu_readings_;
+};
+
+} // namespace frc971::imu
+
+#include "frc971/imu/imu_calibrator-tmpl.h"
+#endif // FRC971_IMU_IMU_CALIBRATOR_H_
diff --git a/frc971/imu/imu_calibrator_solver.cc b/frc971/imu/imu_calibrator_solver.cc
new file mode 100644
index 0000000..19b82e5
--- /dev/null
+++ b/frc971/imu/imu_calibrator_solver.cc
@@ -0,0 +1,89 @@
+#include "frc971/imu/imu_calibrator_solver.h"
+namespace frc971::imu {
+
+struct ImuCalibratorCostFunctor {
+ ImuCalibratorCostFunctor(
+ const std::vector<std::vector<RawImuReading>> &readings,
+ const std::vector<ImuConfig<double>> &nominal_config,
+ const size_t num_residuals)
+ : readings_(readings),
+ nominal_config_(nominal_config),
+ num_residuals_(num_residuals) {}
+ template <typename S>
+ bool operator()(const S *const *const parameters_ptr, S *residual) const {
+ AllParameters<S> params = AllParameters<S>::PopulateFromParameters(
+ nominal_config_, parameters_ptr);
+ std::vector<ImuConfig<S>> imu_configs;
+ for (const auto ¶m : params.imus) {
+ imu_configs.push_back(param);
+ }
+ ImuCalibrator<S> calibrator(imu_configs);
+ for (size_t imu_index = 0; imu_index < readings_.size(); ++imu_index) {
+ const auto imu_readings = readings_[imu_index];
+ for (size_t reading_index = 0; reading_index < imu_readings.size();
+ ++reading_index) {
+ calibrator.InsertImu(imu_index, imu_readings[reading_index]);
+ }
+ }
+ calibrator.CalculateResiduals({residual, num_residuals_});
+ return true;
+ }
+ const std::vector<std::vector<RawImuReading>> readings_;
+ const std::vector<ImuConfig<double>> nominal_config_;
+ const size_t num_residuals_;
+};
+
+AllParameters<double> Solve(
+ const std::vector<std::vector<RawImuReading>> &readings,
+ const std::vector<ImuConfig<double>> &nominal_config) {
+ ceres::Problem problem;
+
+ ceres::EigenQuaternionParameterization *quaternion_local_parameterization =
+ new ceres::EigenQuaternionParameterization();
+ AllParameters<double> parameters;
+ std::vector<size_t> num_readings;
+ CHECK_EQ(nominal_config.size(), readings.size());
+ for (size_t imu_index = 0; imu_index < nominal_config.size(); ++imu_index) {
+ const size_t num_params = readings[imu_index].size();
+ parameters.imus.emplace_back(nominal_config[imu_index]);
+ num_readings.push_back(num_params);
+ }
+ // Set up the only cost function (also known as residual). This uses
+ // auto-differentiation to obtain the derivative (jacobian).
+
+ {
+ const size_t num_residuals =
+ ImuCalibrator<double>::CalculateNumResiduals(num_readings);
+ ceres::DynamicCostFunction *cost_function =
+ new ceres::DynamicAutoDiffCostFunction<ImuCalibratorCostFunctor>(
+ new ImuCalibratorCostFunctor(readings, nominal_config,
+ num_residuals));
+
+ auto [vector_parameters, post_populate_methods] =
+ parameters.PopulateParameters(quaternion_local_parameterization,
+ &problem, cost_function);
+
+ cost_function->SetNumResiduals(num_residuals);
+
+ problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1.0),
+ vector_parameters);
+ for (auto &method : post_populate_methods) {
+ method();
+ }
+ }
+
+ // Run the solver!
+ ceres::Solver::Options options;
+ options.minimizer_progress_to_stdout = true;
+ options.gradient_tolerance = 1e-12;
+ options.function_tolerance = 1e-6;
+ options.parameter_tolerance = 1e-6;
+ ceres::Solver::Summary summary;
+ Solve(options, &problem, &summary);
+ LOG(INFO) << summary.FullReport();
+ LOG(INFO) << "Solution is " << (summary.IsSolutionUsable() ? "" : "NOT ")
+ << "usable";
+ LOG(INFO) << "Solution:\n" << parameters.ToString();
+ return parameters;
+}
+} // namespace frc971::imu
diff --git a/frc971/imu/imu_calibrator_solver.h b/frc971/imu/imu_calibrator_solver.h
new file mode 100644
index 0000000..148fcc4
--- /dev/null
+++ b/frc971/imu/imu_calibrator_solver.h
@@ -0,0 +1,14 @@
+#ifndef FRC971_IMU_IMU_CALIBRATOR_SOLVER_H_
+#define FRC971_IMU_IMU_CALIBRATOR_SOLVER_H_
+
+#include "frc971/imu/imu_calibrator.h"
+
+namespace frc971::imu {
+
+// Stores all the IMU data from a log so that we can feed it into the
+// ImuCalibrator readily.
+AllParameters<double> Solve(
+ const std::vector<std::vector<RawImuReading>> &readings,
+ const std::vector<ImuConfig<double>> &nominal_config);
+} // namespace frc971::imu
+#endif // FRC971_IMU_IMU_CALIBRATOR_SOLVER_H_
diff --git a/frc971/imu/imu_calibrator_test.cc b/frc971/imu/imu_calibrator_test.cc
new file mode 100644
index 0000000..a341b2a
--- /dev/null
+++ b/frc971/imu/imu_calibrator_test.cc
@@ -0,0 +1,215 @@
+#include "frc971/imu/imu_calibrator.h"
+
+#include <random>
+
+#include "gtest/gtest.h"
+
+#include "frc971/imu/imu_calibrator_solver.h"
+
+namespace frc971::imu::testing {
+class ImuSimulator {
+ public:
+ ImuSimulator(const std::vector<ImuConfig<double>> &imu_configs)
+ : imu_configs_(imu_configs), readings_(imu_configs.size()) {}
+ void SimulateForTime(aos::monotonic_clock::duration duration,
+ aos::monotonic_clock::duration dt,
+ const Eigen::Vector3d &gravity_vector,
+ const Eigen::Vector3d &accel,
+ const Eigen::Vector3d &gyro) {
+ for (const aos::monotonic_clock::time_point end_time = now + duration;
+ now < end_time; now += dt) {
+ for (size_t imu_index = 0; imu_index < imu_configs_.size(); ++imu_index) {
+ const ImuConfig<double> &config = imu_configs_[imu_index];
+ const Eigen::Quaterniond rotation =
+ config.is_origin ? Eigen::Quaterniond::Identity()
+ : config.parameters->rotation.inverse();
+ const std::chrono::nanoseconds time_offset{
+ config.is_origin
+ ? 0
+ : static_cast<uint64_t>(config.parameters->time_offset * 1e9)};
+ readings_[imu_index].emplace_back(
+ now + time_offset,
+ rotation * gyro + config.dynamic_params.gyro_zero + GyroNoise(),
+ rotation *
+ (accel + gravity_vector * config.dynamic_params.gravity) +
+ AccelNoise());
+ }
+ }
+ }
+ Eigen::Vector3d GyroNoise() {
+ return (enable_noise_ ? 1.0 : 0.0) *
+ Eigen::Vector3d(distribution_(generator_), distribution_(generator_),
+ distribution_(generator_));
+ }
+ Eigen::Vector3d AccelNoise() { return GyroNoise() * 2.0; }
+ void set_enable_noise(bool enable) { enable_noise_ = enable; }
+ std::vector<std::vector<RawImuReading>> readings() const {
+ return readings_;
+ };
+
+ private:
+ const std::vector<ImuConfig<double>> imu_configs_;
+ std::vector<std::vector<RawImuReading>> readings_;
+ aos::monotonic_clock::time_point now = aos::monotonic_clock::epoch();
+
+ std::mt19937 generator_;
+ std::normal_distribution<> distribution_{0.0, 0.00025};
+ bool enable_noise_ = false;
+};
+
+namespace {
+void VerifyParameters(const std::vector<ImuConfig<double>> &imus,
+ const AllParameters<double> ¶ms, double eps = 1e-8) {
+ ASSERT_EQ(imus.size(), params.imus.size());
+ for (size_t imu_index = 0; imu_index < imus.size(); ++imu_index) {
+ SCOPED_TRACE(imu_index);
+ const ImuConfig<double> &expected = imus[imu_index];
+ const ImuConfig<double> &calculated = params.imus[imu_index];
+ EXPECT_EQ(expected.parameters.has_value(),
+ calculated.parameters.has_value());
+ EXPECT_NEAR(expected.dynamic_params.gravity,
+ calculated.dynamic_params.gravity, eps);
+ EXPECT_LT((expected.dynamic_params.gyro_zero -
+ calculated.dynamic_params.gyro_zero)
+ .norm(),
+ eps)
+ << expected.dynamic_params.gyro_zero.transpose() << " vs. "
+ << calculated.dynamic_params.gyro_zero.transpose();
+ if (expected.parameters.has_value()) {
+ EXPECT_NEAR(expected.parameters->time_offset,
+ calculated.parameters->time_offset, eps);
+ EXPECT_LT(((expected.parameters->rotation *
+ calculated.parameters->rotation.inverse())
+ .coeffs() -
+ Eigen::Quaterniond::Identity().coeffs())
+ .norm(),
+ eps)
+ << expected.parameters->rotation.coeffs().transpose() << " vs. "
+ << calculated.parameters->rotation.coeffs().transpose();
+ }
+ }
+}
+} // namespace
+
+// Confirms that we can calibrate in a relatively simple scenario where we have
+// some gyro/accelerometer offsets and a small rotation that is not accounted
+// for in the nominal parameters.
+TEST(ImuCalibratorTest, BasicCalibrationTest) {
+ std::vector<ImuConfig<double>> nominal_imus = {
+ ImuConfig<double>{true, std::nullopt},
+ ImuConfig<double>{false, std::make_optional<StaticImuParameters<double>>(
+ Eigen::Quaterniond::Identity(), 0.0)}};
+
+ std::vector<ImuConfig<double>> real_imus = nominal_imus;
+ real_imus[0].dynamic_params.gravity = 1.005;
+ real_imus[0].dynamic_params.gyro_zero << 0.001, 0.002, 0.003;
+ real_imus[1].dynamic_params.gyro_zero << -0.009, -0.007, -0.001;
+ real_imus[1].parameters->rotation =
+ Eigen::AngleAxisd(0.01, Eigen::Vector3d::UnitZ());
+ ImuSimulator simulator(real_imus);
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 1, 0), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ simulator.SimulateForTime(
+ std::chrono::seconds(1), std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0.1, 0.2, 0.3),
+ Eigen::Vector3d(1.0, 0.0, 0.0));
+ simulator.SimulateForTime(
+ std::chrono::seconds(1), std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0.1, 0.2, 0.3),
+ Eigen::Vector3d(0.0, 1.0, 0.0));
+ auto params = Solve(simulator.readings(), nominal_imus);
+ LOG(INFO) << params.ToString();
+ LOG(INFO) << real_imus[0].ToString();
+ LOG(INFO) << real_imus[1].ToString();
+ VerifyParameters(real_imus, params);
+}
+
+// Separately test the estimation of the time offset between IMUs.
+// This is done separately because the optimization problem is poorly condition
+// for estimating time offsets when there is just a handful of step changes in
+// the IMU inputs; feeding in a sine wave works much better for allowing the
+// solver to estimate the offset.
+TEST(ImuCalibratorTest, TimeOffsetTest) {
+ gflags::FlagSaver flag_saver;
+
+ std::vector<ImuConfig<double>> nominal_imus = {
+ ImuConfig<double>{true, std::nullopt},
+ ImuConfig<double>{false, std::make_optional<StaticImuParameters<double>>(
+ Eigen::Quaterniond::Identity(), 0.0)}};
+
+ std::vector<ImuConfig<double>> real_imus = nominal_imus;
+ real_imus[1].parameters->time_offset = 0.0255;
+ ImuSimulator simulator(real_imus);
+ // Note on convergence: It is easy to end up in situations where the problem
+ // is not outstandingly well conditioned and we can end up with local minima
+ // where changes to physical calibration attributes can explain the time
+ // offset.
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ for (size_t ii = 0; ii < 10000; ++ii) {
+ simulator.SimulateForTime(
+ std::chrono::milliseconds(1), std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0.0, 0.0, 0.0),
+ Eigen::Vector3d(std::sin(ii / 1000.0), 0.0, 0.0));
+ }
+ auto params = Solve(simulator.readings(), nominal_imus);
+ LOG(INFO) << params.ToString();
+ LOG(INFO) << real_imus[0].ToString();
+ LOG(INFO) << real_imus[1].ToString();
+ VerifyParameters(real_imus, params, 1e-6);
+}
+
+// Test that if we add in some random noise that the solver still behaves
+// itself.
+TEST(ImuCalibratorTest, RandomNoise) {
+ std::vector<ImuConfig<double>> nominal_imus = {
+ ImuConfig<double>{true, std::nullopt},
+ ImuConfig<double>{false, std::make_optional<StaticImuParameters<double>>(
+ Eigen::Quaterniond::Identity(), 0.0)}};
+
+ std::vector<ImuConfig<double>> real_imus = nominal_imus;
+ real_imus[0].dynamic_params.gravity = 0.999;
+ real_imus[0].dynamic_params.gyro_zero << 0.001, 0.002, 0.003;
+ real_imus[1].dynamic_params.gyro_zero << -0.009, -0.007, -0.001;
+ real_imus[1].parameters->rotation =
+ Eigen::AngleAxisd(0.01, Eigen::Vector3d::UnitZ());
+ ImuSimulator simulator(real_imus);
+ simulator.set_enable_noise(true);
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 1, 0), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ simulator.SimulateForTime(std::chrono::seconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 0),
+ Eigen::Vector3d(0.0, 0.0, 0.0));
+ for (size_t ii = 0; ii < 6000; ++ii) {
+ simulator.SimulateForTime(std::chrono::milliseconds(1),
+ std::chrono::milliseconds(1),
+ Eigen::Vector3d(0, 0, 1),
+ Eigen::Vector3d(std::sin(ii / 1000.0), 0.0, 0.0),
+ Eigen::Vector3d(1.0, 0.0, 0.0));
+ }
+ auto params = Solve(simulator.readings(), nominal_imus);
+ LOG(INFO) << params.ToString();
+ LOG(INFO) << real_imus[0].ToString();
+ LOG(INFO) << real_imus[1].ToString();
+ VerifyParameters(real_imus, params, 1e-4);
+}
+} // namespace frc971::imu::testing
diff --git a/frc971/imu_reader/imu_watcher.cc b/frc971/imu_reader/imu_watcher.cc
index 153b84f..ae69996 100644
--- a/frc971/imu_reader/imu_watcher.cc
+++ b/frc971/imu_reader/imu_watcher.cc
@@ -37,13 +37,15 @@
if (zeroer_.Faulted()) {
if (value->checksum_failed()) {
imu_fault_tracker_.pico_to_pi_checksum_mismatch++;
- } else if (value->previous_reading_diag_stat()->checksum_mismatch()) {
+ } else if (value->has_previous_reading_diag_stat() &&
+ value->previous_reading_diag_stat()->checksum_mismatch()) {
imu_fault_tracker_.imu_to_pico_checksum_mismatch++;
} else {
imu_fault_tracker_.other_zeroing_faults++;
}
} else {
- if (!first_valid_data_counter_.has_value()) {
+ if (!first_valid_data_counter_.has_value() &&
+ value->has_data_counter()) {
first_valid_data_counter_ = value->data_counter();
}
}
@@ -68,12 +70,14 @@
}
// Set encoders to nullopt if we are faulted at all (faults may include
// checksum mismatches).
+ const bool have_encoders = !zeroer_.Faulted() &&
+ value->has_left_encoder() &&
+ value->has_right_encoder();
const std::optional<Eigen::Vector2d> encoders =
- zeroer_.Faulted()
- ? std::nullopt
- : std::make_optional(Eigen::Vector2d{
- left_encoder_.Unwrap(value->left_encoder()),
- right_encoder_.Unwrap(value->right_encoder())});
+ have_encoders ? std::make_optional(Eigen::Vector2d{
+ left_encoder_.Unwrap(value->left_encoder()),
+ right_encoder_.Unwrap(value->right_encoder())})
+ : std::nullopt;
{
const aos::monotonic_clock::time_point pi_read_timestamp =
aos::monotonic_clock::time_point(
diff --git a/frc971/orin/argus_camera.cc b/frc971/orin/argus_camera.cc
index 37f7e7d..bb16674 100644
--- a/frc971/orin/argus_camera.cc
+++ b/frc971/orin/argus_camera.cc
@@ -431,7 +431,6 @@
const Argus::CaptureMetadata *metadata = ibuffer->getMetadata();
const Argus::ICaptureMetadata *imetadata =
Argus::interface_cast<const Argus::ICaptureMetadata>(metadata);
- CHECK(imetadata);
return imetadata;
}
@@ -572,45 +571,48 @@
const Argus::ICaptureMetadata *imetadata = buffer.imetadata();
- aos::Sender<frc971::vision::CameraImage>::Builder builder =
- sender.MakeBuilder();
+ if (imetadata) {
+ aos::Sender<frc971::vision::CameraImage>::Builder builder =
+ sender.MakeBuilder();
- uint8_t *data_pointer = nullptr;
- builder.fbb()->StartIndeterminateVector(FLAGS_width * FLAGS_height * 2, 1,
- 64, &data_pointer);
+ uint8_t *data_pointer = nullptr;
+ builder.fbb()->StartIndeterminateVector(FLAGS_width * FLAGS_height * 2,
+ 1, 64, &data_pointer);
- YCbCr422(buffer.nvbuf_surf(), data_pointer);
- flatbuffers::Offset<flatbuffers::Vector<uint8_t>> data_offset =
- builder.fbb()->EndIndeterminateVector(FLAGS_width * FLAGS_height * 2,
- 1);
+ YCbCr422(buffer.nvbuf_surf(), data_pointer);
+ flatbuffers::Offset<flatbuffers::Vector<uint8_t>> data_offset =
+ builder.fbb()->EndIndeterminateVector(
+ FLAGS_width * FLAGS_height * 2, 1);
- auto image_builder = builder.MakeBuilder<frc971::vision::CameraImage>();
- image_builder.add_data(data_offset);
- image_builder.add_rows(FLAGS_height);
- image_builder.add_cols(FLAGS_width);
- {
- aos::ScopedNotRealtime nrt;
- image_builder.add_monotonic_timestamp_ns(
- imetadata->getSensorTimestamp());
+ auto image_builder = builder.MakeBuilder<frc971::vision::CameraImage>();
+ image_builder.add_data(data_offset);
+ image_builder.add_rows(FLAGS_height);
+ image_builder.add_cols(FLAGS_width);
+ {
+ aos::ScopedNotRealtime nrt;
+ image_builder.add_monotonic_timestamp_ns(
+ imetadata->getSensorTimestamp());
+ }
+ builder.CheckOk(builder.Send(image_builder.Finish()));
+
+ const aos::monotonic_clock::time_point after_send =
+ aos::monotonic_clock::now();
+
+ VLOG(1)
+ << "Got " << imetadata->getCaptureId() << " delay "
+ << chrono::duration<double>(
+ chrono::nanoseconds(
+ (buffer.start_time().time_since_epoch().count() -
+ (imetadata->getSensorTimestamp() +
+ imetadata->getFrameReadoutTime()))))
+ .count()
+ << " mmap "
+ << chrono::duration<double>(after_send - buffer.start_time())
+ .count()
+ << "sec dt "
+ << chrono::duration<double>(buffer.start_time() - last_time).count()
+ << "sec, exposure " << imetadata->getSensorExposureTime();
}
- builder.CheckOk(builder.Send(image_builder.Finish()));
-
- const aos::monotonic_clock::time_point after_send =
- aos::monotonic_clock::now();
-
- VLOG(1)
- << "Got " << imetadata->getCaptureId() << " delay "
- << chrono::duration<double>(
- chrono::nanoseconds(
- (buffer.start_time().time_since_epoch().count() -
- (imetadata->getSensorTimestamp() +
- imetadata->getFrameReadoutTime()))))
- .count()
- << " mmap "
- << chrono::duration<double>(after_send - buffer.start_time()).count()
- << "sec dt "
- << chrono::duration<double>(buffer.start_time() - last_time).count()
- << "sec, exposure " << imetadata->getSensorExposureTime();
last_time = buffer.start_time();
timer->Schedule(event_loop.monotonic_now());
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 6cee780..d96822b 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -328,6 +328,7 @@
"//frc971/control_loops/drivetrain:improved_down_estimator",
"//frc971/vision:charuco_lib",
"//frc971/vision:vision_fbs",
+ "//frc971/vision:vision_util_lib",
"//frc971/wpilib:imu_batch_fbs",
"//frc971/wpilib:imu_fbs",
"//third_party:opencv",
@@ -374,3 +375,25 @@
"@com_github_google_glog//:glog",
],
)
+
+cc_test(
+ name = "vision_util_lib_test",
+ srcs = ["vision_util_lib_test.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/testing:googletest",
+ "//frc971/vision:vision_util_lib",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_library(
+ name = "target_map_utils",
+ srcs = ["target_map_utils.cc"],
+ hdrs = ["target_map_utils.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/vision:target_map_fbs",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
diff --git a/frc971/vision/calibration.fbs b/frc971/vision/calibration.fbs
index df8b7b0..64a5d3c 100644
--- a/frc971/vision/calibration.fbs
+++ b/frc971/vision/calibration.fbs
@@ -48,6 +48,12 @@
// 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);
+
+ // The camera number. This may be empty, "0", or "1".
+ camera_number:int (id: 8);
+
+ // The reprojection error associated with the camera calibration.
+ reprojection_error:float (id: 9);
}
// Calibration information for all the cameras we know about.
diff --git a/frc971/vision/intrinsics_calibration.cc b/frc971/vision/intrinsics_calibration.cc
index f98aadb..16a53a7 100644
--- a/frc971/vision/intrinsics_calibration.cc
+++ b/frc971/vision/intrinsics_calibration.cc
@@ -13,15 +13,17 @@
#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.");
+// TODO: Would be nice to remove this, but it depends on year-by-year Constants
DEFINE_string(base_intrinsics, "",
"Intrinsics to use for estimating board pose prior to solving "
"for the new intrinsics.");
+DEFINE_string(calibration_folder, ".", "Folder to place calibration files.");
+DEFINE_string(camera_id, "", "Camera ID in format YY-NN-- year and number.");
+DEFINE_string(channel, "/camera", "Camera channel to use");
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+DEFINE_string(cpu_name, "", "Pi/Orin name to calibrate.");
+DEFINE_bool(display_undistorted, false,
+ "If true, display the undistorted image.");
namespace frc971::vision {
namespace {
@@ -32,7 +34,7 @@
aos::ShmEventLoop event_loop(&config.message());
- std::string hostname = FLAGS_pi;
+ std::string hostname = FLAGS_cpu_name;
if (hostname == "") {
hostname = aos::network::GetHostname();
LOG(INFO) << "Using pi/orin name from hostname as " << hostname;
@@ -41,9 +43,10 @@
<< "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());
+ IntrinsicsCalibration extractor(&event_loop, hostname, FLAGS_channel,
+ FLAGS_camera_id, FLAGS_base_intrinsics,
+ FLAGS_display_undistorted,
+ FLAGS_calibration_folder, exit_handle.get());
event_loop.Run();
diff --git a/frc971/vision/intrinsics_calibration_lib.cc b/frc971/vision/intrinsics_calibration_lib.cc
index a5ffec7..c48c12d 100644
--- a/frc971/vision/intrinsics_calibration_lib.cc
+++ b/frc971/vision/intrinsics_calibration_lib.cc
@@ -1,15 +1,23 @@
#include "frc971/vision/intrinsics_calibration_lib.h"
+DECLARE_bool(visualize);
+
namespace frc971::vision {
+// Found that under 50 ms would fail image too often on intrinsics with
+// visualize on
+constexpr aos::monotonic_clock::duration kMaxImageAge =
+ aos::monotonic_clock::duration(std::chrono::milliseconds(50));
+
IntrinsicsCalibration::IntrinsicsCalibration(
aos::EventLoop *event_loop, std::string_view hostname,
- std::string_view camera_id, std::string_view base_intrinsics_file,
- bool display_undistorted, std::string_view calibration_folder,
- aos::ExitHandle *exit_handle)
+ std::string_view camera_channel, std::string_view camera_id,
+ std::string_view base_intrinsics_file, bool display_undistorted,
+ std::string_view calibration_folder, aos::ExitHandle *exit_handle)
: hostname_(hostname),
cpu_type_(aos::network::ParsePiOrOrin(hostname_)),
cpu_number_(aos::network::ParsePiOrOrinNumber(hostname_)),
+ camera_channel_(camera_channel),
camera_id_(camera_id),
prev_H_camera_board_(Eigen::Affine3d()),
prev_image_H_camera_board_(Eigen::Affine3d()),
@@ -18,7 +26,7 @@
base_intrinsics_file)),
charuco_extractor_(
event_loop, &base_intrinsics_.message(), TargetType::kCharuco,
- "/camera",
+ camera_channel_,
[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,
@@ -30,17 +38,18 @@
image_callback_(
event_loop,
absl::StrCat("/", aos::network::ParsePiOrOrin(hostname_).value(),
- // TODO: Need to make this work with multiple cameras
- std::to_string(cpu_number_.value()), "/camera"),
+ std::to_string(cpu_number_.value()), camera_channel_),
[this](cv::Mat rgb_image,
const aos::monotonic_clock::time_point eof) {
charuco_extractor_.HandleImage(rgb_image, eof);
},
- std::chrono::milliseconds(5)),
+ kMaxImageAge),
display_undistorted_(display_undistorted),
calibration_folder_(calibration_folder),
exit_handle_(exit_handle) {
- LOG(INFO) << "Hostname is: " << hostname_;
+ LOG(INFO) << "Hostname is: " << hostname_ << " and camera channel is "
+ << camera_channel_;
+
CHECK(cpu_number_) << ": Invalid cpu number " << hostname_
<< ", failed to parse cpu number";
std::regex re{"^[0-9][0-9]-[0-9][0-9]"};
@@ -54,25 +63,28 @@
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_visualize) {
+ // 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());
+ 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);
+ 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) {
+ LOG(INFO) << "Skipping because pose is not valid";
return;
}
CHECK(tvecs_eigen.size() == 1)
@@ -99,8 +111,8 @@
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";
+ LOG(INFO) << "Captured: " << all_charuco_ids_.size() << " points; \nMoved "
+ << static_cast<int>(percent_motion * 100) << "% 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
@@ -120,9 +132,11 @@
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)";
+ << "points; \nMoved enough ("
+ << static_cast<int>(percent_motion * 100)
+ << "%); Need to stop (last motion was "
+ << static_cast<int>(percent_stop * 100)
+ << "% of limit; needs to be < 1% to capture)";
}
prev_image_H_camera_board_ = H_camera_board_;
@@ -157,7 +171,9 @@
IntrinsicsCalibration::BuildCalibration(
cv::Mat camera_matrix, cv::Mat dist_coeffs,
aos::realtime_clock::time_point realtime_now, std::string_view cpu_type,
- int cpu_number, std::string_view camera_id, uint16_t team_number) {
+ uint16_t cpu_number, std::string_view camera_channel,
+ std::string_view camera_id, uint16_t team_number,
+ double reprojection_error) {
flatbuffers::FlatBufferBuilder fbb;
flatbuffers::Offset<flatbuffers::String> name_offset =
fbb.CreateString(absl::StrFormat("%s%d", cpu_type, cpu_number));
@@ -169,6 +185,9 @@
reinterpret_cast<double *>(camera_matrix.data)[i]);
});
+ std::optional<uint16_t> camera_number =
+ frc971::vision::CameraNumberFromChannel(std::string(camera_channel));
+
flatbuffers::Offset<flatbuffers::Vector<float>>
distortion_coefficients_offset =
fbb.CreateVector<float>(5u, [&dist_coeffs](size_t i) {
@@ -180,7 +199,10 @@
camera_calibration_builder.add_node_name(name_offset);
camera_calibration_builder.add_team_number(team_number);
+ camera_calibration_builder.add_camera_number(camera_number.value());
camera_calibration_builder.add_camera_id(camera_id_offset);
+ camera_calibration_builder.add_reprojection_error(
+ static_cast<float>(reprojection_error));
camera_calibration_builder.add_calibration_timestamp(
realtime_now.time_since_epoch().count());
camera_calibration_builder.add_intrinsics(intrinsics_offset);
@@ -209,7 +231,7 @@
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)
+ CHECK_LE(reprojection_error, 2.0)
<< ": Reproduction error is bad-- greater than 1 pixel.";
LOG(INFO) << "Reprojection Error is " << reprojection_error;
@@ -222,15 +244,23 @@
aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
camera_calibration = BuildCalibration(
camera_matrix, dist_coeffs, realtime_now, cpu_type_.value(),
- cpu_number_.value(), camera_id_, team_number.value());
+ cpu_number_.value(), camera_channel_, camera_id_,
+ team_number.value(), reprojection_error);
std::stringstream time_ss;
time_ss << realtime_now;
+ std::string camera_number_optional = "";
+ std::optional<uint16_t> camera_number =
+ frc971::vision::CameraNumberFromChannel(camera_channel_);
+ if (camera_number != std::nullopt) {
+ camera_number_optional = "-" + std::to_string(camera_number.value());
+ }
const std::string calibration_filename =
calibration_folder_ +
- absl::StrFormat("/calibration_%s-%d-%d_cam-%s_%s.json",
+ absl::StrFormat("/calibration_%s-%d-%d%s_cam-%s_%s.json",
cpu_type_.value(), team_number.value(),
- cpu_number_.value(), camera_id_, time_ss.str());
+ cpu_number_.value(), camera_number_optional, camera_id_,
+ time_ss.str());
LOG(INFO) << calibration_filename << " -> "
<< aos::FlatbufferToJson(camera_calibration,
diff --git a/frc971/vision/intrinsics_calibration_lib.h b/frc971/vision/intrinsics_calibration_lib.h
index 7f08138..605741f 100644
--- a/frc971/vision/intrinsics_calibration_lib.h
+++ b/frc971/vision/intrinsics_calibration_lib.h
@@ -15,12 +15,14 @@
#include "aos/time/time.h"
#include "aos/util/file.h"
#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/vision_util_lib.h"
namespace frc971::vision {
class IntrinsicsCalibration {
public:
IntrinsicsCalibration(aos::EventLoop *event_loop, std::string_view hostname,
+ std::string_view camera_channel,
std::string_view camera_id,
std::string_view base_intrinsics_file,
bool display_undistorted,
@@ -39,8 +41,9 @@
static aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
BuildCalibration(cv::Mat camera_matrix, cv::Mat dist_coeffs,
aos::realtime_clock::time_point realtime_now,
- std::string_view cpu_type, int cpu_number,
- std::string_view camera_id, uint16_t team_number);
+ std::string_view cpu_type, uint16_t cpu_number,
+ std::string_view camera_channel, std::string_view camera_id,
+ uint16_t team_number, double reprojection_error);
private:
static constexpr double kDeltaRThreshold = M_PI / 6.0;
@@ -52,6 +55,7 @@
std::string hostname_;
const std::optional<std::string_view> cpu_type_;
const std::optional<uint16_t> cpu_number_;
+ const std::string camera_channel_;
const std::string camera_id_;
std::vector<std::vector<int>> all_charuco_ids_;
diff --git a/y2023/localizer/utils.cc b/frc971/vision/target_map_utils.cc
similarity index 81%
rename from y2023/localizer/utils.cc
rename to frc971/vision/target_map_utils.cc
index 7faca1f..232c2b6 100644
--- a/y2023/localizer/utils.cc
+++ b/frc971/vision/target_map_utils.cc
@@ -1,6 +1,6 @@
-#include "y2023/localizer/utils.h"
+#include "frc971/vision/target_map_utils.h"
-namespace y2023::localizer {
+namespace frc971::vision {
Eigen::Matrix<double, 4, 4> PoseToTransform(
const frc971::vision::TargetPoseFbs *pose) {
const frc971::vision::Position *position = pose->position();
@@ -11,4 +11,4 @@
quaternion->z()))
.matrix();
}
-} // namespace y2023::localizer
+} // namespace frc971::vision
diff --git a/frc971/vision/target_map_utils.h b/frc971/vision/target_map_utils.h
new file mode 100644
index 0000000..36b4e30
--- /dev/null
+++ b/frc971/vision/target_map_utils.h
@@ -0,0 +1,14 @@
+#ifndef FRC971_VISION_TARGET_MAP_UTILS_H_
+#define FRC971_VISION_TARGET_MAP_UTILS_H_
+
+#include <Eigen/Dense>
+
+#include "frc971/vision/target_map_generated.h"
+
+namespace frc971::vision {
+// Converts a TargetPoseFbs into a transformation matrix.
+Eigen::Matrix<double, 4, 4> PoseToTransform(
+ const frc971::vision::TargetPoseFbs *pose);
+} // namespace frc971::vision
+
+#endif // FRC971_VISION_TARGET_MAP_UTILS_H_
diff --git a/frc971/vision/vision_util_lib.cc b/frc971/vision/vision_util_lib.cc
index b65e883..bfd6209 100644
--- a/frc971/vision/vision_util_lib.cc
+++ b/frc971/vision/vision_util_lib.cc
@@ -43,4 +43,19 @@
return result;
}
+std::optional<uint16_t> CameraNumberFromChannel(std::string camera_channel) {
+ if (camera_channel.find("/camera") == std::string::npos) {
+ return std::nullopt;
+ }
+ // If the string doesn't end in /camera#, return nullopt
+ uint16_t cam_len = std::string("/camera").length();
+ if (camera_channel.length() != camera_channel.find("/camera") + cam_len + 1) {
+ return std::nullopt;
+ }
+
+ uint16_t camera_number = std::stoi(
+ camera_channel.substr(camera_channel.find("/camera") + cam_len, 1));
+ return camera_number;
+}
+
} // namespace frc971::vision
diff --git a/frc971/vision/vision_util_lib.h b/frc971/vision/vision_util_lib.h
index 6fb32eb..8ce651c 100644
--- a/frc971/vision/vision_util_lib.h
+++ b/frc971/vision/vision_util_lib.h
@@ -7,16 +7,24 @@
#include "frc971/vision/calibration_generated.h"
+// Extract the CameraExtrinsics from a CameraCalibration struct
namespace frc971::vision {
std::optional<cv::Mat> CameraExtrinsics(
const frc971::vision::calibration::CameraCalibration *camera_calibration);
+// Extract the CameraIntrinsics from a CameraCalibration struct
cv::Mat CameraIntrinsics(
const frc971::vision::calibration::CameraCalibration *camera_calibration);
+// Extract the CameraDistCoeffs from a CameraCalibration struct
cv::Mat CameraDistCoeffs(
const frc971::vision::calibration::CameraCalibration *camera_calibration);
+// Get the camera number from a camera channel name, e.g., return 2 from
+// "/camera2". Returns nullopt if string doesn't start with "/camera" or does
+// not have a number
+std::optional<uint16_t> CameraNumberFromChannel(std::string camera_channel);
+
} // namespace frc971::vision
#endif // FRC971_VISION_VISION_UTIL_LIB_H_
diff --git a/frc971/vision/vision_util_lib_test.cc b/frc971/vision/vision_util_lib_test.cc
new file mode 100644
index 0000000..ff9c0a3
--- /dev/null
+++ b/frc971/vision/vision_util_lib_test.cc
@@ -0,0 +1,15 @@
+#include "frc971/vision/vision_util_lib.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971::vision {
+// For now, just testing extracting camera number from channel name
+TEST(VisionUtilsTest, CameraNumberFromChannel) {
+ ASSERT_EQ(CameraNumberFromChannel("/camera0").value(), 0);
+ ASSERT_EQ(CameraNumberFromChannel("/camera1").value(), 1);
+ ASSERT_EQ(CameraNumberFromChannel("/camera"), std::nullopt);
+ ASSERT_EQ(CameraNumberFromChannel("/orin1/camera0").value(), 0);
+ ASSERT_EQ(CameraNumberFromChannel("/orin1/camera1").value(), 1);
+ ASSERT_EQ(CameraNumberFromChannel("/orin1"), std::nullopt);
+}
+} // namespace frc971::vision
diff --git a/scouting/scouting_test.cy.js b/scouting/scouting_test.cy.js
index 7d5d75b..691d8fc 100644
--- a/scouting/scouting_test.cy.js
+++ b/scouting/scouting_test.cy.js
@@ -84,14 +84,14 @@
cy.get('[type="radio"]').first().check();
clickButton('Start Match');
- // Pick and Place Cone in Auto.
- clickButton('CONE');
- clickButton('HIGH');
+ // Pick and Place Note in Auto.
+ clickButton('NOTE');
+ clickButton('AMP');
// Pick and Place Cube in Teleop.
clickButton('Start Teleop');
- clickButton('CUBE');
- clickButton('LOW');
+ clickButton('NOTE');
+ clickButton('AMP AMPLIFIED');
// Robot dead and revive.
clickButton('DEAD');
@@ -99,19 +99,19 @@
// Endgame.
clickButton('Endgame');
- cy.contains(/Docked & Engaged/).click();
+ cy.contains(/Harmony/).click();
clickButton('End Match');
headerShouldBe(teamNumber + ' Review and Submit ');
cy.get('#review_data li')
.eq(0)
.should('have.text', ' Started match at position 1 ');
- cy.get('#review_data li').eq(1).should('have.text', ' Picked up kCone ');
+ cy.get('#review_data li').eq(1).should('have.text', 'Picked up Note');
cy.get('#review_data li')
.last()
.should(
'have.text',
- ' Ended Match; docked: false, engaged: true, attempted to dock and engage: false '
+ ' Ended Match; park: false, onStage: false, harmony: true, trapNote: false '
);
clickButton('Submit');
@@ -264,8 +264,8 @@
cy.get('[type="radio"]').first().check();
clickButton('Start Match');
- // Pick up cone.
- clickButton('CONE');
+ // Pick up note.
+ clickButton('NOTE');
// Undo that pick up.
clickButton('UNDO');
@@ -274,8 +274,8 @@
headerShouldBe('3990 Pickup ');
// Check the same thing but for undoing place.
- clickButton('CUBE');
- clickButton('MID');
+ clickButton('NOTE');
+ clickButton('AMP');
clickButton('UNDO');
headerShouldBe('3990 Place ');
});
diff --git a/scouting/webserver/requests/messages/submit_2024_actions.fbs b/scouting/webserver/requests/messages/submit_2024_actions.fbs
index 5cb3cbe..61e36bc 100644
--- a/scouting/webserver/requests/messages/submit_2024_actions.fbs
+++ b/scouting/webserver/requests/messages/submit_2024_actions.fbs
@@ -15,7 +15,9 @@
mobility:bool (id:0);
}
-table PenaltyAction {}
+table PenaltyAction {
+ penalties: int (id:0);
+}
table PickupNoteAction {
auto:bool (id:0);
diff --git a/scouting/webserver/requests/requests.go b/scouting/webserver/requests/requests.go
index 75b438b..31ad4e3 100644
--- a/scouting/webserver/requests/requests.go
+++ b/scouting/webserver/requests/requests.go
@@ -199,8 +199,7 @@
}
func (handler requestAllMatchesHandler) teamHasBeenDataScouted(key MatchAssemblyKey, teamNumber string) (bool, error) {
- // TODO change this to reference 2024 stats
- stats, err := handler.db.ReturnStats2023ForTeam(
+ stats, err := handler.db.ReturnStats2024ForTeam(
teamNumber, key.MatchNumber, key.SetNumber, key.CompLevel, false)
if err != nil {
return false, err
@@ -480,7 +479,7 @@
} else if action_type == submit_2024_actions.ActionTypePenaltyAction {
var penaltyAction submit_2024_actions.PenaltyAction
penaltyAction.Init(actionTable.Bytes, actionTable.Pos)
- stat.Penalties += 1
+ stat.Penalties += penaltyAction.Penalties()
} else if action_type == submit_2024_actions.ActionTypePickupNoteAction {
var pick_up_action submit_2024_actions.PickupNoteAction
@@ -1165,12 +1164,12 @@
err = handler.db.AddToStats2024(stats)
if err != nil {
- respondWithError(w, http.StatusInternalServerError, fmt.Sprint("Failed to submit stats: ", stats, ": ", err))
+ respondWithError(w, http.StatusInternalServerError, fmt.Sprint("Failed to submit stats2024: ", stats, ": ", err))
return
}
builder := flatbuffers.NewBuilder(50 * 1024)
- builder.Finish((&SubmitActionsResponseT{}).Pack(builder))
+ builder.Finish((&Submit2024ActionsResponseT{}).Pack(builder))
w.Write(builder.FinishedBytes())
}
diff --git a/scouting/webserver/requests/requests_test.go b/scouting/webserver/requests/requests_test.go
index 67244d3..eb5e904 100644
--- a/scouting/webserver/requests/requests_test.go
+++ b/scouting/webserver/requests/requests_test.go
@@ -131,30 +131,22 @@
},
},
// Pretend that we have some data scouting data.
- stats2023: []db.Stats2023{
+ stats2024: []db.Stats2024{
{
- TeamNumber: "5", MatchNumber: 1, SetNumber: 1,
- CompLevel: "qm", StartingQuadrant: 3, LowCubesAuto: 10,
- MiddleCubesAuto: 1, HighCubesAuto: 1, CubesDroppedAuto: 0,
- LowConesAuto: 1, MiddleConesAuto: 2, HighConesAuto: 1,
- ConesDroppedAuto: 0, LowCubes: 1, MiddleCubes: 1,
- HighCubes: 2, CubesDropped: 1, LowCones: 1,
- MiddleCones: 2, HighCones: 0, ConesDropped: 1, SuperchargedPieces: 0,
- AvgCycle: 34, Mobility: false, DockedAuto: true, EngagedAuto: true,
- BalanceAttemptAuto: false, Docked: false, Engaged: false,
- BalanceAttempt: false, CollectedBy: "alex",
+ PreScouting: false, TeamNumber: "5",
+ MatchNumber: 1, SetNumber: 1, CompLevel: "qm", StartingQuadrant: 3,
+ SpeakerAuto: 2, AmpAuto: 4, NotesDroppedAuto: 1, MobilityAuto: true,
+ Speaker: 0, Amp: 1, SpeakerAmplified: 2, AmpAmplified: 1,
+ NotesDropped: 0, Penalties: 01, TrapNote: true, AvgCycle: 233,
+ Park: false, OnStage: true, Harmony: false, CollectedBy: "alex",
},
{
- TeamNumber: "973", MatchNumber: 3, SetNumber: 1,
- CompLevel: "qm", StartingQuadrant: 1, LowCubesAuto: 0,
- MiddleCubesAuto: 1, HighCubesAuto: 1, CubesDroppedAuto: 2,
- LowConesAuto: 0, MiddleConesAuto: 0, HighConesAuto: 0,
- ConesDroppedAuto: 1, LowCubes: 0, MiddleCubes: 0,
- HighCubes: 1, CubesDropped: 0, LowCones: 0,
- MiddleCones: 2, HighCones: 1, ConesDropped: 1, SuperchargedPieces: 0,
- AvgCycle: 53, Mobility: true, DockedAuto: true, EngagedAuto: false,
- BalanceAttemptAuto: false, Docked: false, Engaged: false,
- BalanceAttempt: true, CollectedBy: "bob",
+ PreScouting: false, TeamNumber: "973",
+ MatchNumber: 3, SetNumber: 1, CompLevel: "qm", StartingQuadrant: 1,
+ SpeakerAuto: 0, AmpAuto: 2, NotesDroppedAuto: 0, MobilityAuto: false,
+ Speaker: 0, Amp: 4, SpeakerAmplified: 3, AmpAmplified: 1,
+ NotesDropped: 0, Penalties: 1, TrapNote: true, AvgCycle: 120,
+ Park: true, OnStage: false, Harmony: false, CollectedBy: "bob",
},
},
}
@@ -416,8 +408,10 @@
},
{
ActionTaken: &submit_2024_actions.ActionTypeT{
- Type: submit_2024_actions.ActionTypePenaltyAction,
- Value: &submit_2024_actions.PenaltyActionT{},
+ Type: submit_2024_actions.ActionTypePenaltyAction,
+ Value: &submit_2024_actions.PenaltyActionT{
+ Penalties: 5,
+ },
},
Timestamp: 2400,
},
@@ -485,7 +479,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 2,
SpeakerAuto: 0, AmpAuto: 1, NotesDroppedAuto: 1, MobilityAuto: true,
Speaker: 0, Amp: 0, SpeakerAmplified: 1, AmpAmplified: 1,
- NotesDropped: 0, Penalties: 1, TrapNote: false, AvgCycle: 950,
+ NotesDropped: 0, Penalties: 5, TrapNote: false, AvgCycle: 950,
Park: false, OnStage: false, Harmony: true, CollectedBy: "",
}
diff --git a/scouting/www/BUILD b/scouting/www/BUILD
index 55e9a8f..a403bf3 100644
--- a/scouting/www/BUILD
+++ b/scouting/www/BUILD
@@ -29,12 +29,12 @@
name = "static_files",
app_files = ":app",
pictures = [
- "//third_party/y2023/field:pictures",
+ "//third_party/y2024/field:pictures",
],
replace_prefixes = {
"prod": "",
"dev": "",
- "third_party/y2023": "pictures",
+ "third_party/y2024": "pictures",
},
tags = [
"no-remote-cache",
diff --git a/scouting/www/entry/BUILD b/scouting/www/entry/BUILD
index 2884f23..98b457b 100644
--- a/scouting/www/entry/BUILD
+++ b/scouting/www/entry/BUILD
@@ -12,7 +12,7 @@
":node_modules/@angular/forms",
"//scouting/webserver/requests/messages:error_response_ts_fbs",
"//scouting/webserver/requests/messages:request_all_matches_response_ts_fbs",
- "//scouting/webserver/requests/messages:submit_actions_ts_fbs",
+ "//scouting/webserver/requests/messages:submit_2024_actions_ts_fbs",
"//scouting/www/rpc",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
],
diff --git a/scouting/www/entry/entry.component.css b/scouting/www/entry/entry.component.css
index 38e9072..246887c 100644
--- a/scouting/www/entry/entry.component.css
+++ b/scouting/www/entry/entry.component.css
@@ -11,15 +11,10 @@
touch-action: manipulation;
}
-#switchFldbtn {
- width: 15%;
- display: block;
- margin: 0 auto;
- box-shadow: 2px 2px 1px #ccc;
- max-width: 105px;
- text-align: center;
+.row ul div span {
+ padding: 0px;
}
-.row ul div span {
+input label {
padding: 0px;
}
diff --git a/scouting/www/entry/entry.component.ts b/scouting/www/entry/entry.component.ts
index 5a93251..b3e1e92 100644
--- a/scouting/www/entry/entry.component.ts
+++ b/scouting/www/entry/entry.component.ts
@@ -11,19 +11,19 @@
import {Builder, ByteBuffer} from 'flatbuffers';
import {ErrorResponse} from '../../webserver/requests/messages/error_response_generated';
import {
- ObjectType,
- ScoreLevel,
- SubmitActions,
StartMatchAction,
+ ScoreType,
+ StageType,
+ Submit2024Actions,
MobilityAction,
- AutoBalanceAction,
- PickupObjectAction,
- PlaceObjectAction,
+ PenaltyAction,
+ PickupNoteAction,
+ PlaceNoteAction,
RobotDeathAction,
EndMatchAction,
ActionType,
Action,
-} from '../../webserver/requests/messages/submit_actions_generated';
+} from '../../webserver/requests/messages/submit_2024_actions_generated';
import {Match} from '../../webserver/requests/messages/request_all_matches_response_generated';
import {MatchListRequestor} from '@org_frc971/scouting/www/rpc';
@@ -62,23 +62,14 @@
mobility: boolean;
}
| {
- type: 'autoBalanceAction';
+ type: 'pickupNoteAction';
timestamp?: number;
- docked: boolean;
- engaged: boolean;
- balanceAttempt: boolean;
- }
- | {
- type: 'pickupObjectAction';
- timestamp?: number;
- objectType: ObjectType;
auto?: boolean;
}
| {
- type: 'placeObjectAction';
+ type: 'placeNoteAction';
timestamp?: number;
- objectType?: ObjectType;
- scoreLevel: ScoreLevel;
+ scoreType: ScoreType;
auto?: boolean;
}
| {
@@ -87,10 +78,14 @@
robotOn: boolean;
}
| {
+ type: 'penaltyAction';
+ timestamp?: number;
+ penalties: number;
+ }
+ | {
type: 'endMatchAction';
- docked: boolean;
- engaged: boolean;
- balanceAttempt: boolean;
+ stageType: StageType;
+ trapNote: boolean;
timestamp?: number;
}
| {
@@ -98,6 +93,12 @@
// It is used for undoing purposes.
type: 'endAutoPhase';
timestamp?: number;
+ }
+ | {
+ // This is not a action that is submitted,
+ // It is used for undoing purposes.
+ type: 'endTeleopPhase';
+ timestamp?: number;
};
@Component({
@@ -110,8 +111,7 @@
// of radio buttons.
readonly COMP_LEVELS = COMP_LEVELS;
readonly COMP_LEVEL_LABELS = COMP_LEVEL_LABELS;
- readonly ObjectType = ObjectType;
- readonly ScoreLevel = ScoreLevel;
+ readonly ScoreType = ScoreType;
section: Section = 'Team Selection';
@Input() matchNumber: number = 1;
@@ -127,10 +127,10 @@
errorMessage: string = '';
autoPhase: boolean = true;
mobilityCompleted: boolean = false;
- lastObject: ObjectType = null;
preScouting: boolean = false;
matchStartTimestamp: number = 0;
+ penalties: number = 0;
teamSelectionIsValid = false;
@@ -192,6 +192,20 @@
return false;
}
+ addPenalty(): void {
+ this.penalties += 1;
+ }
+
+ removePenalty(): void {
+ if (this.penalties > 0) {
+ this.penalties -= 1;
+ }
+ }
+
+ addPenalties(): void {
+ this.addAction({type: 'penaltyAction', penalties: this.penalties});
+ }
+
addAction(action: ActionT): void {
if (action.type == 'startMatchAction') {
// Unix nanosecond timestamp.
@@ -202,27 +216,17 @@
action.timestamp = Date.now() * 1e6 - this.matchStartTimestamp;
}
+ if (action.type == 'endMatchAction') {
+ // endMatchAction occurs at the same time as penaltyAction so add to its timestamp to make it unique.
+ action.timestamp += 1;
+ }
+
if (action.type == 'mobilityAction') {
this.mobilityCompleted = true;
}
- if (action.type == 'autoBalanceAction') {
- // Timestamp is a unique index in the database so
- // adding one makes sure it dosen't overlap with the
- // start teleop action that is added at the same time.
- action.timestamp += 1;
- }
-
- if (
- action.type == 'pickupObjectAction' ||
- action.type == 'placeObjectAction'
- ) {
+ if (action.type == 'pickupNoteAction' || action.type == 'placeNoteAction') {
action.auto = this.autoPhase;
- if (action.type == 'pickupObjectAction') {
- this.lastObject = action.objectType;
- } else if (action.type == 'placeObjectAction') {
- action.objectType = this.lastObject;
- }
}
this.actionList.push(action);
}
@@ -233,14 +237,23 @@
switch (lastAction?.type) {
case 'endAutoPhase':
this.autoPhase = true;
- case 'pickupObjectAction':
+ this.section = 'Pickup';
+ case 'pickupNoteAction':
this.section = 'Pickup';
break;
- case 'placeObjectAction':
+ case 'endTeleopPhase':
+ this.section = 'Pickup';
+ break;
+ case 'placeNoteAction':
this.section = 'Place';
break;
case 'endMatchAction':
- this.section = 'Pickup';
+ this.section = 'Endgame';
+ case 'mobilityAction':
+ this.mobilityCompleted = false;
+ break;
+ case 'startMatchAction':
+ this.section = 'Init';
break;
case 'robotDeathAction':
// TODO(FILIP): Return user to the screen they
@@ -254,12 +267,12 @@
}
}
- stringifyObjectType(objectType: ObjectType): String {
- return ObjectType[objectType];
+ stringifyScoreType(scoreType: ScoreType): String {
+ return ScoreType[scoreType];
}
- stringifyScoreLevel(scoreLevel: ScoreLevel): String {
- return ScoreLevel[scoreLevel];
+ stringifyStageType(stageType: StageType): String {
+ return StageType[stageType];
}
changeSectionTo(target: Section) {
@@ -276,7 +289,7 @@
this.header.nativeElement.scrollIntoView();
}
- async submitActions() {
+ async submit2024Actions() {
const builder = new Builder();
const actionOffsets: number[] = [];
@@ -307,49 +320,42 @@
mobilityActionOffset
);
break;
- case 'autoBalanceAction':
- const autoBalanceActionOffset =
- AutoBalanceAction.createAutoBalanceAction(
- builder,
- action.docked,
- action.engaged,
- action.balanceAttempt
- );
+ case 'penaltyAction':
+ const penaltyActionOffset = PenaltyAction.createPenaltyAction(
+ builder,
+ action.penalties
+ );
actionOffset = Action.createAction(
builder,
BigInt(action.timestamp || 0),
- ActionType.AutoBalanceAction,
- autoBalanceActionOffset
+ ActionType.PenaltyAction,
+ penaltyActionOffset
);
break;
-
- case 'pickupObjectAction':
- const pickupObjectActionOffset =
- PickupObjectAction.createPickupObjectAction(
+ case 'pickupNoteAction':
+ const pickupNoteActionOffset =
+ PickupNoteAction.createPickupNoteAction(
builder,
- action.objectType,
action.auto || false
);
actionOffset = Action.createAction(
builder,
BigInt(action.timestamp || 0),
- ActionType.PickupObjectAction,
- pickupObjectActionOffset
+ ActionType.PickupNoteAction,
+ pickupNoteActionOffset
);
break;
- case 'placeObjectAction':
- const placeObjectActionOffset =
- PlaceObjectAction.createPlaceObjectAction(
- builder,
- action.objectType,
- action.scoreLevel,
- action.auto || false
- );
+ case 'placeNoteAction':
+ const placeNoteActionOffset = PlaceNoteAction.createPlaceNoteAction(
+ builder,
+ action.scoreType,
+ action.auto || false
+ );
actionOffset = Action.createAction(
builder,
BigInt(action.timestamp || 0),
- ActionType.PlaceObjectAction,
- placeObjectActionOffset
+ ActionType.PlaceNoteAction,
+ placeNoteActionOffset
);
break;
@@ -367,9 +373,8 @@
case 'endMatchAction':
const endMatchActionOffset = EndMatchAction.createEndMatchAction(
builder,
- action.docked,
- action.engaged,
- action.balanceAttempt
+ action.stageType,
+ action.trapNote
);
actionOffset = Action.createAction(
builder,
@@ -383,6 +388,10 @@
// Not important action.
break;
+ case 'endTeleopPhase':
+ // Not important action.
+ break;
+
default:
throw new Error(`Unknown action type`);
}
@@ -394,21 +403,21 @@
const teamNumberFb = builder.createString(this.teamNumber);
const compLevelFb = builder.createString(this.compLevel);
- const actionsVector = SubmitActions.createActionsListVector(
+ const actionsVector = Submit2024Actions.createActionsListVector(
builder,
actionOffsets
);
- SubmitActions.startSubmitActions(builder);
- SubmitActions.addTeamNumber(builder, teamNumberFb);
- SubmitActions.addMatchNumber(builder, this.matchNumber);
- SubmitActions.addSetNumber(builder, this.setNumber);
- SubmitActions.addCompLevel(builder, compLevelFb);
- SubmitActions.addActionsList(builder, actionsVector);
- SubmitActions.addPreScouting(builder, this.preScouting);
- builder.finish(SubmitActions.endSubmitActions(builder));
+ Submit2024Actions.startSubmit2024Actions(builder);
+ Submit2024Actions.addTeamNumber(builder, teamNumberFb);
+ Submit2024Actions.addMatchNumber(builder, this.matchNumber);
+ Submit2024Actions.addSetNumber(builder, this.setNumber);
+ Submit2024Actions.addCompLevel(builder, compLevelFb);
+ Submit2024Actions.addActionsList(builder, actionsVector);
+ Submit2024Actions.addPreScouting(builder, this.preScouting);
+ builder.finish(Submit2024Actions.endSubmit2024Actions(builder));
const buffer = builder.asUint8Array();
- const res = await fetch('/requests/submit/submit_actions', {
+ const res = await fetch('/requests/submit/submit_2024_actions', {
method: 'POST',
body: buffer,
});
diff --git a/scouting/www/entry/entry.ng.html b/scouting/www/entry/entry.ng.html
index 43575cd..9490237 100644
--- a/scouting/www/entry/entry.ng.html
+++ b/scouting/www/entry/entry.ng.html
@@ -81,7 +81,7 @@
<h2>Select Starting Position</h2>
<img
id="field_starting_positions_image"
- src="/sha256/b71def525fb78486617a8b350c0ba6907e8ea25f78d4084a932cba8ae922528c/pictures/field/field.jpg"
+ src="/sha256/bb83d2c976c1496bb470371821d1d1882d6baf31178009a6f6cba579880c6a03/pictures/field/2024_field.png"
alt="Starting Positions Image"
class="img-fluid"
/>
@@ -129,15 +129,9 @@
</button>
<button
class="btn btn-warning"
- (click)="changeSectionTo('Place'); addAction({type: 'pickupObjectAction', objectType: ObjectType.kCone});"
+ (click)="changeSectionTo('Place'); addAction({type: 'pickupNoteAction'});"
>
- CONE
- </button>
- <button
- class="btn btn-primary"
- (click)="changeSectionTo('Place'); addAction({type: 'pickupObjectAction', objectType: ObjectType.kCube});"
- >
- CUBE
+ NOTE
</button>
<button
*ngIf="autoPhase && !mobilityCompleted"
@@ -146,49 +140,35 @@
>
Mobility
</button>
- <!-- 'Balancing' during auto. -->
- <div *ngIf="autoPhase" class="d-grid gap-2">
- <label>
- <input
- #docked
- type="radio"
- id="option1"
- name="docked_engaged"
- value="docked"
- />
- Docked (on the charging station)
- </label>
- <label>
- <input
- #engaged
- type="radio"
- id="option2"
- name="docked_engaged"
- value="dockedengaged"
- />
- Docked & Engaged (level & station lights on)
- </label>
- <label>
- <input
- #attempted
- type="radio"
- id="option3"
- name="docked_engaged"
- value="failed"
- />
- Attempted to dock and engage but failed
- </label>
+ <div style="display: flex">
+ <h5>Penalties :</h5>
<button
- class="btn btn-dark"
- (click)="autoPhase = false; addAction({type: 'endAutoPhase'}); addAction({type: 'autoBalanceAction', docked: docked.checked, engaged: engaged.checked, balanceAttempt: attempted.checked});"
+ class="btn-light"
+ style="width: 40px; margin-right: 15px"
+ (click)="removePenalty()"
>
- Start Teleop
+ -
+ </button>
+ <p>{{this.penalties}}</p>
+ <button
+ class="btn-light"
+ style="width: 40px; margin-left: 15px"
+ (click)="addPenalty()"
+ >
+ +
</button>
</div>
<button
+ *ngIf="autoPhase"
+ class="btn btn-dark"
+ (click)="autoPhase = false; addAction({type: 'endAutoPhase'});"
+ >
+ Start Teleop
+ </button>
+ <button
*ngIf="!autoPhase"
class="btn btn-info"
- (click)="changeSectionTo('Endgame')"
+ (click)="changeSectionTo('Endgame'); addAction({type: 'endTeleopPhase'});"
>
Endgame
</button>
@@ -212,23 +192,62 @@
>
DEAD
</button>
+ <div *ngIf="!autoPhase" class="d-grid gap-1" style="padding: 0">
+ <div
+ style="
+ display: flex-wrap;
+ padding: 0;
+ justify-content: center;
+ text-align: center;
+ align-content: center;
+ margin: 0;
+ "
+ >
+ <button
+ class="btn btn-success"
+ (click)="changeSectionTo('Pickup'); addAction({type: 'placeNoteAction', scoreType: ScoreType.kAMP});"
+ style="width: 48%; height: 12vh; margin: 0px 10px 10px 0px"
+ >
+ AMP
+ </button>
+
+ <button
+ class="btn btn-warning"
+ (click)="changeSectionTo('Pickup'); addAction({type: 'placeNoteAction', scoreType: ScoreType.kAMP_AMPLIFIED});"
+ style="width: 48%; height: 12vh; margin: 0px 0px 10px 0px"
+ >
+ AMP AMPLIFIED
+ </button>
+ <button
+ class="btn btn-success"
+ (click)="changeSectionTo('Pickup'); addAction({type: 'placeNoteAction', scoreType: ScoreType.kSPEAKER});"
+ style="width: 48%; height: 12vh; margin: 0px 10px 0px 0px"
+ >
+ SPEAKER
+ </button>
+ <button
+ class="btn btn-warning"
+ (click)="changeSectionTo('Pickup'); addAction({type: 'placeNoteAction', scoreType: ScoreType.kSPEAKER_AMPLIFIED});"
+ style="width: 48%; height: 12vh; margin: 0px 0px 0px 0px"
+ >
+ SPEAKER AMPLIFIED
+ </button>
+ </div>
+ </div>
+
<button
+ *ngIf="autoPhase"
class="btn btn-success"
- (click)="changeSectionTo('Pickup'); addAction({type: 'placeObjectAction', scoreLevel: ScoreLevel.kHigh});"
+ (click)="changeSectionTo('Pickup'); addAction({type: 'placeNoteAction', scoreType: ScoreType.kAMP});"
>
- HIGH
+ AMP
</button>
<button
+ *ngIf="autoPhase"
class="btn btn-warning"
- (click)="changeSectionTo('Pickup'); addAction({type: 'placeObjectAction', scoreLevel: ScoreLevel.kMiddle});"
+ (click)="changeSectionTo('Pickup'); addAction({type: 'placeNoteAction', scoreType: ScoreType.kSPEAKER});"
>
- MID
- </button>
- <button
- class="btn btn-danger"
- (click)="changeSectionTo('Pickup'); addAction({type: 'placeObjectAction', scoreLevel: ScoreLevel.kLow});"
- >
- LOW
+ SPEAKER
</button>
<button
*ngIf="autoPhase && !mobilityCompleted"
@@ -237,58 +256,35 @@
>
Mobility
</button>
- <!-- Impossible to place supercharged pieces in auto. -->
- <div *ngIf="autoPhase == false" class="d-grid gap-2">
+ <div style="display: flex">
+ <h5>Penalties :</h5>
<button
- class="btn btn-dark"
- (click)="changeSectionTo('Pickup'); addAction({type: 'placeObjectAction', scoreLevel: ScoreLevel.kSupercharged});"
+ class="btn-light"
+ style="width: 40px; margin-right: 15px"
+ (click)="removePenalty()"
>
- SUPERCHARGED
+ -
</button>
- </div>
- <!-- 'Balancing' during auto. -->
- <div *ngIf="autoPhase" class="d-grid gap-1">
- <label>
- <input
- #docked
- type="radio"
- id="option1"
- name="docked_engaged"
- value="docked"
- />
- Docked (on the charging station)
- </label>
- <label>
- <input
- #engaged
- type="radio"
- id="option2"
- name="docked_engaged"
- value="dockedengaged"
- />
- Docked & Engaged (level & station lights on)
- </label>
- <label>
- <input
- #attempted
- type="radio"
- id="option3"
- name="docked_engaged"
- value="failed"
- />
- Attempted to dock and engage but failed
- </label>
+ <p>{{this.penalties}}</p>
<button
- class="btn btn-dark"
- (click)="autoPhase = false; addAction({type: 'endAutoPhase'}); addAction({type: 'autoBalanceAction', docked: docked.checked, engaged: engaged.checked, balanceAttempt: attempted.checked});"
+ class="btn-light"
+ style="width: 40px; margin-left: 15px"
+ (click)="addPenalty()"
>
- Start Teleop
+ +
</button>
</div>
<button
+ class="btn btn-dark"
+ *ngIf="autoPhase"
+ (click)="autoPhase = false; addAction({type: 'endAutoPhase'});"
+ >
+ Start Teleop
+ </button>
+ <button
*ngIf="!autoPhase"
class="btn btn-info"
- (click)="changeSectionTo('Endgame')"
+ (click)="changeSectionTo('Endgame'); addAction({type: 'endTeleopPhase'});"
>
Endgame
</button>
@@ -298,7 +294,7 @@
<h6 class="text-muted">
Last Action: {{actionList[actionList.length - 1].type}}
</h6>
- <div class="d-grid gap-5">
+ <div class="d-grid gap-4">
<button class="btn btn-secondary" (click)="undoLastAction()">UNDO</button>
<button
class="btn btn-danger"
@@ -306,40 +302,68 @@
>
DEAD
</button>
- <label>
+ <label style="padding: 0">
<input
- #docked
+ #park
type="radio"
id="option1"
- name="docked_engaged"
- value="docked"
+ name="endgameaction"
+ value="park"
/>
- Docked (on the charging station)
+ Park
</label>
- <label>
+ <label style="padding: 0">
<input
- #engaged
+ #onStage
type="radio"
id="option2"
- name="docked_engaged"
- value="dockedengaged"
+ name="endgameaction"
+ value="onStage"
/>
- Docked & Engaged (level & station lights on)
+ On Stage
</label>
- <label>
+ <label style="padding: 0">
<input
- #attempted
+ #harmony
type="radio"
id="option3"
- name="docked_engaged"
- value="failed"
+ name="endgameaction"
+ value="harmony"
/>
- Attempted to dock and engage but failed
+ Harmony
</label>
+ <label style="padding: 0">
+ <input
+ #trapNote
+ type="checkbox"
+ id="trapnote"
+ name="trapnote"
+ value="trapNote"
+ />
+ Trap Note
+ </label>
+ <div style="display: flex">
+ <h5>Penalties :</h5>
+ <button
+ class="btn-light"
+ style="width: 40px; margin-right: 15px"
+ (click)="removePenalty()"
+ >
+ -
+ </button>
+ <p>{{this.penalties}}</p>
+ <button
+ class="btn-light"
+ style="width: 40px; margin-left: 15px"
+ (click)="addPenalty()"
+ >
+ +
+ </button>
+ </div>
<button
*ngIf="!autoPhase"
class="btn btn-info"
- (click)="changeSectionTo('Review and Submit'); addAction({type: 'endMatchAction', docked: docked.checked, engaged: engaged.checked, balanceAttempt: attempted.checked});"
+ (click)="changeSectionTo('Review and Submit'); addPenalties(); addAction({type: 'endMatchAction', park: park.checked, onStage: onStage.checked, harmony: harmony.checked, trapNote: trapNote.checked});"
>
End Match
</button>
@@ -354,12 +378,6 @@
>
Revive
</button>
- <button
- class="btn btn-info"
- (click)="changeSectionTo('Review and Submit'); addAction({type: 'endMatchAction', docked: docked.checked, engaged: engaged.checked});"
- >
- End Match
- </button>
</div>
</div>
<div *ngSwitchCase="'Review and Submit'" id="Review" class="container-fluid">
@@ -374,21 +392,14 @@
<span *ngSwitchCase="'startMatchAction'">
Started match at position {{action.position}}
</span>
- <span *ngSwitchCase="'pickupObjectAction'">
- Picked up {{stringifyObjectType(action.objectType)}}
- </span>
- <span *ngSwitchCase="'placeObjectAction'">
- Placed at {{stringifyScoreLevel(action.scoreLevel)}}
- </span>
- <span *ngSwitchCase="'autoBalanceAction'">
- Docked: {{action.docked}}, engaged: {{action.engaged}}, attempted
- to balance and engage: {{action.balanceAttempt}}
+ <span *ngSwitchCase="'pickupNoteAction'">Picked up Note</span>
+ <span *ngSwitchCase="'placeNoteAction'">
+ Placed at {{stringifyScoreType(action.scoreType)}}
</span>
<span *ngSwitchCase="'endAutoPhase'">Ended auto phase</span>
<span *ngSwitchCase="'endMatchAction'">
- Ended Match; docked: {{action.docked}}, engaged:
- {{action.engaged}}, attempted to dock and engage:
- {{action.balanceAttempt}}
+ Ended Match; park: {{action.park}}, onStage: {{action.onStage}},
+ harmony: {{action.harmony}}, trapNote: {{action.trapNote}}
</span>
<span *ngSwitchCase="'robotDeathAction'">
Robot on: {{action.robotOn}}
@@ -397,13 +408,18 @@
Mobility: {{action.mobility}}
</span>
<span *ngSwitchDefault>{{action.type}}</span>
+ <span *ngSwitchCase="'penaltyAction'">
+ Penalties: {{action.penalties}}
+ </span>
</div>
</li>
</ul>
</div>
<div class="d-grid gap-5">
<button class="btn btn-secondary" (click)="undoLastAction()">UNDO</button>
- <button class="btn btn-warning" (click)="submitActions();">Submit</button>
+ <button class="btn btn-warning" (click)="submit2024Actions();">
+ Submit
+ </button>
</div>
</div>
<div *ngSwitchCase="'Success'" id="Success" class="container-fluid">
diff --git a/scouting/www/pit_scouting/pit_scouting.component.ts b/scouting/www/pit_scouting/pit_scouting.component.ts
index 58e7647..7bb884c 100644
--- a/scouting/www/pit_scouting/pit_scouting.component.ts
+++ b/scouting/www/pit_scouting/pit_scouting.component.ts
@@ -57,6 +57,8 @@
const builder = new Builder();
const teamNumber = builder.createString(this.teamNumber);
const pitImage = builder.createString(
+ // Remove anything between /s in order to end up with only the file name.
+ // Example : path/to/file.txt -> file.txt
this.pitImage.toString().replace(/^.*[\\\/]/, '')
);
const imageData = SubmitPitImage.createImageDataVector(
diff --git a/scouting/www/rpc/BUILD b/scouting/www/rpc/BUILD
index ab28581..d1367ea 100644
--- a/scouting/www/rpc/BUILD
+++ b/scouting/www/rpc/BUILD
@@ -11,8 +11,8 @@
generate_public_api = False,
deps = [
"//scouting/webserver/requests/messages:error_response_ts_fbs",
- "//scouting/webserver/requests/messages:request_2023_data_scouting_response_ts_fbs",
- "//scouting/webserver/requests/messages:request_2023_data_scouting_ts_fbs",
+ "//scouting/webserver/requests/messages:request_2024_data_scouting_response_ts_fbs",
+ "//scouting/webserver/requests/messages:request_2024_data_scouting_ts_fbs",
"//scouting/webserver/requests/messages:request_all_driver_rankings_response_ts_fbs",
"//scouting/webserver/requests/messages:request_all_driver_rankings_ts_fbs",
"//scouting/webserver/requests/messages:request_all_matches_response_ts_fbs",
diff --git a/scouting/www/rpc/view_data_requestor.ts b/scouting/www/rpc/view_data_requestor.ts
index f1f2b79..74fc212 100644
--- a/scouting/www/rpc/view_data_requestor.ts
+++ b/scouting/www/rpc/view_data_requestor.ts
@@ -11,16 +11,16 @@
Ranking,
RequestAllDriverRankingsResponse,
} from '../../webserver/requests/messages/request_all_driver_rankings_response_generated';
-import {Request2023DataScouting} from '../../webserver/requests/messages/request_2023_data_scouting_generated';
+import {Request2024DataScouting} from '../../webserver/requests/messages/request_2024_data_scouting_generated';
import {
PitImage,
RequestAllPitImagesResponse,
} from '../../webserver/requests/messages/request_all_pit_images_response_generated';
import {RequestAllPitImages} from '../../webserver/requests/messages/request_all_pit_images_generated';
import {
- Stats2023,
- Request2023DataScoutingResponse,
-} from '../../webserver/requests/messages/request_2023_data_scouting_response_generated';
+ Stats2024,
+ Request2024DataScoutingResponse,
+} from '../../webserver/requests/messages/request_2024_data_scouting_response_generated';
@Injectable({providedIn: 'root'})
export class ViewDataRequestor {
@@ -82,15 +82,15 @@
return driverRankingList;
}
// Returns all data scouting entries from the database.
- async fetchStats2023List(): Promise<Stats2023[]> {
+ async fetchStats2024List(): Promise<Stats2024[]> {
let fbBuffer = await this.fetchFromServer(
- Request2023DataScouting.startRequest2023DataScouting,
- Request2023DataScouting.endRequest2023DataScouting,
- '/requests/request/2023_data_scouting'
+ Request2024DataScouting.startRequest2024DataScouting,
+ Request2024DataScouting.endRequest2024DataScouting,
+ '/requests/request/2024_data_scouting'
);
const parsedResponse =
- Request2023DataScoutingResponse.getRootAsRequest2023DataScoutingResponse(
+ Request2024DataScoutingResponse.getRootAsRequest2024DataScoutingResponse(
fbBuffer
);
diff --git a/scouting/www/view/BUILD b/scouting/www/view/BUILD
index 67c7e3b..738ea00 100644
--- a/scouting/www/view/BUILD
+++ b/scouting/www/view/BUILD
@@ -10,11 +10,11 @@
],
deps = [
":node_modules/@angular/forms",
- "//scouting/webserver/requests/messages:delete_2023_data_scouting_response_ts_fbs",
- "//scouting/webserver/requests/messages:delete_2023_data_scouting_ts_fbs",
+ "//scouting/webserver/requests/messages:delete_2024_data_scouting_response_ts_fbs",
+ "//scouting/webserver/requests/messages:delete_2024_data_scouting_ts_fbs",
"//scouting/webserver/requests/messages:error_response_ts_fbs",
- "//scouting/webserver/requests/messages:request_2023_data_scouting_response_ts_fbs",
- "//scouting/webserver/requests/messages:request_2023_data_scouting_ts_fbs",
+ "//scouting/webserver/requests/messages:request_2024_data_scouting_response_ts_fbs",
+ "//scouting/webserver/requests/messages:request_2024_data_scouting_ts_fbs",
"//scouting/webserver/requests/messages:request_all_driver_rankings_response_ts_fbs",
"//scouting/webserver/requests/messages:request_all_driver_rankings_ts_fbs",
"//scouting/webserver/requests/messages:request_all_notes_response_ts_fbs",
diff --git a/scouting/www/view/view.component.ts b/scouting/www/view/view.component.ts
index 64b0680..ea9a61f 100644
--- a/scouting/www/view/view.component.ts
+++ b/scouting/www/view/view.component.ts
@@ -6,9 +6,9 @@
RequestAllDriverRankingsResponse,
} from '../../webserver/requests/messages/request_all_driver_rankings_response_generated';
import {
- Stats2023,
- Request2023DataScoutingResponse,
-} from '../../webserver/requests/messages/request_2023_data_scouting_response_generated';
+ Stats2024,
+ Request2024DataScoutingResponse,
+} from '../../webserver/requests/messages/request_2024_data_scouting_response_generated';
import {
PitImage,
@@ -19,12 +19,12 @@
Note,
RequestAllNotesResponse,
} from '../../webserver/requests/messages/request_all_notes_response_generated';
-import {Delete2023DataScouting} from '../../webserver/requests/messages/delete_2023_data_scouting_generated';
-import {Delete2023DataScoutingResponse} from '../../webserver/requests/messages/delete_2023_data_scouting_response_generated';
+import {Delete2024DataScouting} from '../../webserver/requests/messages/delete_2024_data_scouting_generated';
+import {Delete2024DataScoutingResponse} from '../../webserver/requests/messages/delete_2024_data_scouting_response_generated';
import {ViewDataRequestor} from '../rpc';
-type Source = 'Notes' | 'Stats2023' | 'PitImages' | 'DriverRanking';
+type Source = 'Notes' | 'Stats2024' | 'PitImages' | 'DriverRanking';
//TODO(Filip): Deduplicate
const COMP_LEVEL_LABELS = {
@@ -63,7 +63,7 @@
noteList: Note[] = [];
driverRankingList: Ranking[] = [];
pitImageList: PitImage[][] = [];
- statList: Stats2023[] = [];
+ statList: Stats2024[] = [];
// Fetch notes on initialization.
ngOnInit() {
@@ -123,8 +123,8 @@
this.fetchNotes();
}
- case 'Stats2023': {
- this.fetchStats2023();
+ case 'Stats2024': {
+ this.fetchStats2024();
}
case 'PitImages': {
@@ -162,7 +162,7 @@
}
// Gets called when a user clicks the delete icon.
- async deleteDataScouting(
+ async delete2024DataScouting(
compLevel: string,
matchNumber: number,
setNumber: number,
@@ -172,17 +172,17 @@
'block_alerts'
) as HTMLInputElement;
if (block_alerts.checked || window.confirm('Actually delete data?')) {
- await this.requestDeleteDataScouting(
+ await this.requestDelete2024DataScouting(
compLevel,
matchNumber,
setNumber,
teamNumber
);
- await this.fetchStats2023();
+ await this.fetchStats2024();
}
}
- async requestDeleteDataScouting(
+ async requestDelete2024DataScouting(
compLevel: string,
matchNumber: number,
setNumber: number,
@@ -194,7 +194,7 @@
const teamNumberData = builder.createString(teamNumber);
builder.finish(
- Delete2023DataScouting.createDelete2023DataScouting(
+ Delete2024DataScouting.createDelete2024DataScouting(
builder,
compLevelData,
matchNumber,
@@ -204,7 +204,7 @@
);
const buffer = builder.asUint8Array();
- const res = await fetch('/requests/delete/delete_2023_data_scouting', {
+ const res = await fetch('/requests/delete/delete_2024_data_scouting', {
method: 'POST',
body: buffer,
});
@@ -270,12 +270,12 @@
}
// Fetch all data scouting (stats) data and store in statList.
- async fetchStats2023() {
+ async fetchStats2024() {
this.progressMessage = 'Fetching stats list. Please be patient.';
this.errorMessage = '';
try {
- this.statList = await this.viewDataRequestor.fetchStats2023List();
+ this.statList = await this.viewDataRequestor.fetchStats2024List();
this.progressMessage = 'Successfully fetched stats list.';
} catch (e) {
this.errorMessage = e;
diff --git a/scouting/www/view/view.ng.html b/scouting/www/view/view.ng.html
index 239548c..14e8c8a 100644
--- a/scouting/www/view/view.ng.html
+++ b/scouting/www/view/view.ng.html
@@ -24,10 +24,10 @@
<a
class="dropdown-item"
href="#"
- (click)="switchDataSource('Stats2023')"
+ (click)="switchDataSource('Stats2024')"
id="stats_source_dropdown"
>
- Stats
+ Stats2024
</a>
</li>
<li>
@@ -93,8 +93,8 @@
</tbody>
</table>
</div>
- <!-- Stats Data Display. -->
- <div *ngSwitchCase="'Stats2023'">
+ <!-- Stats2024 Data Display. -->
+ <div *ngSwitchCase="'Stats2024'">
<table class="table">
<thead>
<tr>
@@ -114,17 +114,17 @@
</tr>
</thead>
<tbody>
- <tr *ngFor="let stat2023 of statList; index as i;">
- <th scope="row">{{stat2023.matchNumber()}}</th>
- <td>{{stat2023.teamNumber()}}</td>
- <td>{{COMP_LEVEL_LABELS[stat2023.compLevel()]}}</td>
- <td>{{stat2023.collectedBy()}}</td>
+ <tr *ngFor="let stat2024 of statList; index as i;">
+ <th scope="row">{{stat2024.matchNumber()}}</th>
+ <td>{{stat2024.teamNumber()}}</td>
+ <td>{{COMP_LEVEL_LABELS[stat2024.compLevel()]}}</td>
+ <td>{{stat2024.collectedBy()}}</td>
<!-- Delete Icon. -->
<td>
<button
class="btn btn-danger"
id="delete_button_{{i}}"
- (click)="deleteDataScouting(stat2023.compLevel(), stat2023.matchNumber(), stat2023.setNumber(), stat2023.teamNumber())"
+ (click)="delete2024DataScouting(stat2024.compLevel(), stat2024.matchNumber(), stat2024.setNumber(), stat2024.teamNumber())"
>
<i class="bi bi-trash"></i>
</button>
diff --git a/third_party/flatbuffers/build_defs.bzl b/third_party/flatbuffers/build_defs.bzl
index 5f6d71b..da26cec 100644
--- a/third_party/flatbuffers/build_defs.bzl
+++ b/third_party/flatbuffers/build_defs.bzl
@@ -96,7 +96,7 @@
mnemonic = "Flatc",
progress_message = "Generating flatbuffer files for %{input}:",
)
- return [DefaultInfo(files = depset(outs), runfiles = ctx.runfiles(files = outs)), FlatbufferLibraryInfo(srcs = ctx.files.srcs)]
+ return [DefaultInfo(files = depset(outs)), FlatbufferLibraryInfo(srcs = ctx.files.srcs)]
_flatbuffer_library_compile = rule(
implementation = _flatbuffer_library_compile_impl,
diff --git a/third_party/flatbuffers/include/flatbuffers/flatbuffer_builder.h b/third_party/flatbuffers/include/flatbuffers/flatbuffer_builder.h
index efa4d89..6f9d7c8 100644
--- a/third_party/flatbuffers/include/flatbuffers/flatbuffer_builder.h
+++ b/third_party/flatbuffers/include/flatbuffers/flatbuffer_builder.h
@@ -787,7 +787,7 @@
/// where the vector is stored.
template<typename T>
Offset<Vector<const T *>> CreateVectorOfStructs(const T *v, size_t len) {
- StartVector(len * sizeof(T) / AlignOf<T>(), sizeof(T), AlignOf<T>());
+ StartVector(len, sizeof(T), AlignOf<T>());
if (len > 0) {
PushBytes(reinterpret_cast<const uint8_t *>(v), sizeof(T) * len);
}
@@ -1211,7 +1211,7 @@
// Allocates space for a vector of structures.
// Must be completed with EndVectorOfStructs().
template<typename T> T *StartVectorOfStructs(size_t vector_size) {
- StartVector(vector_size * sizeof(T) / AlignOf<T>(), sizeof(T), AlignOf<T>());
+ StartVector(vector_size, sizeof(T), AlignOf<T>());
return reinterpret_cast<T *>(buf_.make_space(vector_size * sizeof(T)));
}
diff --git a/third_party/flatbuffers/include/flatbuffers/reflection.h b/third_party/flatbuffers/include/flatbuffers/reflection.h
index 1aa0863..84213c7 100644
--- a/third_party/flatbuffers/include/flatbuffers/reflection.h
+++ b/third_party/flatbuffers/include/flatbuffers/reflection.h
@@ -508,14 +508,19 @@
// root should point to the root type for this flatbuffer.
// buf should point to the start of flatbuffer data.
// length specifies the size of the flatbuffer data.
+// Returns true if the flatbuffer is valid. Returns false if either:
+// * The flatbuffer is incorrectly constructed (e.g., it points to memory
+// locations outside of the current memory buffer).
+// * The flatbuffer is too complex, and the flatbuffer verifier chosen to bail
+// when attempting to traverse the tree of tables.
bool Verify(const reflection::Schema &schema, const reflection::Object &root,
const uint8_t *buf, size_t length, uoffset_t max_depth = 64,
- uoffset_t max_tables = 1000000);
+ uoffset_t max_tables = 3000000);
bool VerifySizePrefixed(const reflection::Schema &schema,
const reflection::Object &root, const uint8_t *buf,
size_t length, uoffset_t max_depth = 64,
- uoffset_t max_tables = 1000000);
+ uoffset_t max_tables = 3000000);
} // namespace flatbuffers
diff --git a/third_party/flatbuffers/include/flatbuffers/reflection_generated.h b/third_party/flatbuffers/include/flatbuffers/reflection_generated.h
index 6a99e66..b340086 100644
--- a/third_party/flatbuffers/include/flatbuffers/reflection_generated.h
+++ b/third_party/flatbuffers/include/flatbuffers/reflection_generated.h
@@ -523,6 +523,7 @@
int64_t value = 0;
flatbuffers::unique_ptr<reflection::TypeT> union_type{};
std::vector<std::string> documentation{};
+ std::vector<flatbuffers::unique_ptr<reflection::KeyValueT>> attributes{};
EnumValT() = default;
EnumValT(const EnumValT &o);
EnumValT(EnumValT&&) FLATBUFFERS_NOEXCEPT = default;
@@ -603,6 +604,9 @@
const flatbuffers::Vector<flatbuffers::Offset<reflection::KeyValue>> *attributes() const {
return GetPointer<const flatbuffers::Vector<flatbuffers::Offset<reflection::KeyValue>> *>(VT_ATTRIBUTES);
}
+ flatbuffers::Vector<flatbuffers::Offset<reflection::KeyValue>> *mutable_attributes() {
+ return GetPointer<flatbuffers::Vector<flatbuffers::Offset<reflection::KeyValue>> *>(VT_ATTRIBUTES);
+ }
void clear_attributes() {
ClearField(VT_ATTRIBUTES);
}
@@ -2462,7 +2466,8 @@
(lhs.name == rhs.name) &&
(lhs.value == rhs.value) &&
((lhs.union_type == rhs.union_type) || (lhs.union_type && rhs.union_type && *lhs.union_type == *rhs.union_type)) &&
- (lhs.documentation == rhs.documentation);
+ (lhs.documentation == rhs.documentation) &&
+ (lhs.attributes.size() == rhs.attributes.size() && std::equal(lhs.attributes.cbegin(), lhs.attributes.cend(), rhs.attributes.cbegin(), [](flatbuffers::unique_ptr<reflection::KeyValueT> const &a, flatbuffers::unique_ptr<reflection::KeyValueT> const &b) { return (a == b) || (a && b && *a == *b); }));
}
inline bool operator!=(const EnumValT &lhs, const EnumValT &rhs) {
@@ -2475,6 +2480,8 @@
value(o.value),
union_type((o.union_type) ? new reflection::TypeT(*o.union_type) : nullptr),
documentation(o.documentation) {
+ attributes.reserve(o.attributes.size());
+ for (const auto &attributes_ : o.attributes) { attributes.emplace_back((attributes_) ? new reflection::KeyValueT(*attributes_) : nullptr); }
}
inline EnumValT &EnumValT::operator=(EnumValT o) FLATBUFFERS_NOEXCEPT {
@@ -2482,6 +2489,7 @@
std::swap(value, o.value);
std::swap(union_type, o.union_type);
std::swap(documentation, o.documentation);
+ std::swap(attributes, o.attributes);
return *this;
}
@@ -2498,6 +2506,7 @@
{ auto _e = value(); _o->value = _e; }
{ auto _e = union_type(); if (_e) { if(_o->union_type) { _e->UnPackTo(_o->union_type.get(), _resolver); } else { _o->union_type = flatbuffers::unique_ptr<reflection::TypeT>(_e->UnPack(_resolver)); } } else if (_o->union_type) { _o->union_type.reset(); } }
{ auto _e = documentation(); if (_e) { _o->documentation.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->documentation[_i] = _e->Get(_i)->str(); } } else { _o->documentation.resize(0); } }
+ { auto _e = attributes(); if (_e) { _o->attributes.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { if(_o->attributes[_i]) { _e->Get(_i)->UnPackTo(_o->attributes[_i].get(), _resolver); } else { _o->attributes[_i] = flatbuffers::unique_ptr<reflection::KeyValueT>(_e->Get(_i)->UnPack(_resolver)); }; } } else { _o->attributes.resize(0); } }
}
inline flatbuffers::Offset<EnumVal> EnumVal::Pack(flatbuffers::FlatBufferBuilder &_fbb, const EnumValT* _o, const flatbuffers::rehasher_function_t *_rehasher) {
@@ -2512,12 +2521,14 @@
auto _value = _o->value;
auto _union_type = _o->union_type ? CreateType(_fbb, _o->union_type.get(), _rehasher) : 0;
auto _documentation = _fbb.CreateVectorOfStrings(_o->documentation);
+ auto _attributes = _fbb.CreateVector<flatbuffers::Offset<reflection::KeyValue>> (_o->attributes.size(), [](size_t i, _VectorArgs *__va) { return CreateKeyValue(*__va->__fbb, __va->__o->attributes[i].get(), __va->__rehasher); }, &_va );
return reflection::CreateEnumVal(
_fbb,
_name,
_value,
_union_type,
- _documentation);
+ _documentation,
+ _attributes);
}
@@ -3211,21 +3222,24 @@
{ flatbuffers::ET_LONG, 0, -1 },
{ flatbuffers::ET_SEQUENCE, 0, 0 },
{ flatbuffers::ET_SEQUENCE, 0, 1 },
- { flatbuffers::ET_STRING, 1, -1 }
+ { flatbuffers::ET_STRING, 1, -1 },
+ { flatbuffers::ET_SEQUENCE, 1, 2 }
};
static const flatbuffers::TypeFunction type_refs[] = {
reflection::ObjectTypeTable,
- reflection::TypeTypeTable
+ reflection::TypeTypeTable,
+ reflection::KeyValueTypeTable
};
static const char * const names[] = {
"name",
"value",
"object",
"union_type",
- "documentation"
+ "documentation",
+ "attributes"
};
static const flatbuffers::TypeTable tt = {
- flatbuffers::ST_TABLE, 5, type_codes, type_refs, nullptr, nullptr, names
+ flatbuffers::ST_TABLE, 6, type_codes, type_refs, nullptr, nullptr, names
};
return &tt;
}
diff --git a/third_party/y2024/field/2024_field.png b/third_party/y2024/field/2024_field.png
new file mode 100644
index 0000000..c79e929
--- /dev/null
+++ b/third_party/y2024/field/2024_field.png
Binary files differ
diff --git a/third_party/y2024/field/BUILD b/third_party/y2024/field/BUILD
index 04346f5..04db591 100644
--- a/third_party/y2024/field/BUILD
+++ b/third_party/y2024/field/BUILD
@@ -2,9 +2,10 @@
name = "pictures",
srcs = [
# Picture from the FIRST inspires field drawings.
+ "2024.png",
# https://www.firstinspires.org/robotics/frc/playing-field
# Copyright 2024 FIRST
- "2024.png",
+ "2024_field.png",
],
visibility = ["//visibility:public"],
)
\ No newline at end of file
diff --git a/y2023/localizer/BUILD b/y2023/localizer/BUILD
index eb0f886..9b35fcf 100644
--- a/y2023/localizer/BUILD
+++ b/y2023/localizer/BUILD
@@ -81,26 +81,15 @@
)
cc_library(
- name = "utils",
- srcs = ["utils.cc"],
- hdrs = ["utils.h"],
- visibility = ["//visibility:public"],
- deps = [
- "//frc971/vision:target_map_fbs",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
name = "map_expander_lib",
srcs = ["map_expander_lib.cc"],
hdrs = ["map_expander_lib.h"],
deps = [
":relative_scoring_map_fbs",
":scoring_map_fbs",
- ":utils",
"//aos:flatbuffers",
"//aos:json_to_flatbuffer",
+ "//frc971/vision:target_map_utils",
],
)
@@ -138,7 +127,6 @@
visibility = ["//visibility:public"],
deps = [
":status_fbs",
- ":utils",
":visualization_fbs",
"//aos/containers:sized_array",
"//aos/events:event_loop",
@@ -152,6 +140,7 @@
"//frc971/control_loops/drivetrain/localization:utils",
"//frc971/imu_reader:imu_watcher",
"//frc971/vision:target_map_fbs",
+ "//frc971/vision:target_map_utils",
"//y2023:constants",
"//y2023/constants:constants_fbs",
],
@@ -164,12 +153,12 @@
deps = [
":localizer",
":status_fbs",
- ":utils",
"//aos/events:simulated_event_loop",
"//aos/events/logging:log_writer",
"//aos/testing:googletest",
"//frc971/control_loops/drivetrain:drivetrain_test_lib",
"//frc971/control_loops/drivetrain:localizer_fbs",
+ "//frc971/vision:target_map_utils",
"//y2023/constants:simulated_constants_sender",
"//y2023/control_loops/drivetrain:drivetrain_base",
],
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index 76784fb..6f9a16b 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -5,8 +5,8 @@
#include "aos/containers/sized_array.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/pose.h"
+#include "frc971/vision/target_map_utils.h"
#include "y2023/constants.h"
-#include "y2023/localizer/utils.h"
DEFINE_double(max_pose_error, 1e-6,
"Throw out target poses with a higher pose error than this");
diff --git a/y2023/localizer/localizer_test.cc b/y2023/localizer/localizer_test.cc
index fb0efa9..c47e46e 100644
--- a/y2023/localizer/localizer_test.cc
+++ b/y2023/localizer/localizer_test.cc
@@ -8,10 +8,10 @@
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/pose.h"
#include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/target_map_utils.h"
#include "y2023/constants/simulated_constants_sender.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
#include "y2023/localizer/status_generated.h"
-#include "y2023/localizer/utils.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
diff --git a/y2023/localizer/map_expander_lib.cc b/y2023/localizer/map_expander_lib.cc
index 5a94985..884b184 100644
--- a/y2023/localizer/map_expander_lib.cc
+++ b/y2023/localizer/map_expander_lib.cc
@@ -1,6 +1,6 @@
#include "y2023/localizer/map_expander_lib.h"
-#include "y2023/localizer/utils.h"
+#include "frc971/vision/target_map_utils.h"
namespace y2023::localizer {
namespace {
diff --git a/y2023/localizer/utils.h b/y2023/localizer/utils.h
deleted file mode 100644
index 8241cf8..0000000
--- a/y2023/localizer/utils.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef Y2023_LOCALIZER_UTILS_H_
-#define Y2023_LOCALIZER_UTILS_H_
-
-#include <Eigen/Dense>
-
-#include "frc971/vision/target_map_generated.h"
-
-namespace y2023::localizer {
-// Converts a TargetPoseFbs into a transformation matrix.
-Eigen::Matrix<double, 4, 4> PoseToTransform(
- const frc971::vision::TargetPoseFbs *pose);
-} // namespace y2023::localizer
-
-#endif // Y2023_LOCALIZER_UTILS_H_
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index f583abd..88c1f54 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -78,9 +78,9 @@
"//aos/events:shm_event_loop",
"//frc971/constants:constants_sender_lib",
"//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/vision:target_map_utils",
"//frc971/vision:vision_fbs",
"//y2023/localizer",
- "//y2023/localizer:utils",
"//y2023/vision:vision_util",
"@com_google_absl//absl/strings",
],
diff --git a/y2023/vision/localization_verifier.cc b/y2023/vision/localization_verifier.cc
index c16f874..01ef9fe 100644
--- a/y2023/vision/localization_verifier.cc
+++ b/y2023/vision/localization_verifier.cc
@@ -3,9 +3,9 @@
#include "aos/init.h"
#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/vision/target_map_utils.h"
#include "frc971/vision/vision_generated.h"
#include "y2023/localizer/localizer.h"
-#include "y2023/localizer/utils.h"
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
@@ -49,7 +49,7 @@
}
const Localizer::Transform H_camera_target =
- localizer::PoseToTransform(target_pose);
+ frc971::vision::PoseToTransform(target_pose);
const Localizer::Transform H_field_robot =
LocalizerOutputToTransform(*localizer_fetcher->get());
diff --git a/y2024/constants.h b/y2024/constants.h
index 8eac0c9..1d0f5d3 100644
--- a/y2024/constants.h
+++ b/y2024/constants.h
@@ -59,13 +59,6 @@
return (16.0 / 64.0) * (18.0 / 62.0);
}
- static constexpr double kIntakePivotPotRatio() { return 16.0 / 64.0; }
-
- static constexpr double kIntakePivotPotRadiansPerVolt() {
- return kIntakePivotPotRatio() * (3.0 /*turns*/ / 5.0 /*volts*/) *
- (2 * M_PI /*radians*/);
- }
-
static constexpr double kMaxIntakePivotEncoderPulsesPerSecond() {
return control_loops::superstructure::intake_pivot::kFreeSpeed /
(2.0 * M_PI) *
@@ -102,6 +95,12 @@
subsystem_params;
double potentiometer_offset;
};
+
+ struct AbsoluteEncoderConstants {
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
+ subsystem_params;
+ };
};
// Creates and returns a Values instance for the constants.
diff --git a/y2024/constants/7971.json b/y2024/constants/7971.json
index 9e6bafb..d2e766a 100644
--- a/y2024/constants/7971.json
+++ b/y2024/constants/7971.json
@@ -3,15 +3,12 @@
{
"robot": {
- "intake_constants": {
- {% set _ = intake_pivot_zero.update(
- {
- "measured_absolute_position" : 0.0
- }
- ) %}
- "zeroing_constants": {{ intake_pivot_zero | tojson(indent=2)}},
- "potentiometer_offset": 0.0
- },
+ {% set _ = intake_pivot_zero.update(
+ {
+ "measured_absolute_position" : 0.0
+ }
+ ) %}
+ "intake_constants": {{ intake_pivot_zero | tojson(indent=2)}},
"climber_constants": {
{% set _ = climber_zero.update(
{
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
index 641783f..9ab6ad6 100644
--- a/y2024/constants/971.json
+++ b/y2024/constants/971.json
@@ -8,15 +8,12 @@
}
],
"robot": {
- "intake_constants": {
- {% set _ = intake_pivot_zero.update(
- {
- "measured_absolute_position" : 0.0
- }
- ) %}
- "zeroing_constants": {{ intake_pivot_zero | tojson(indent=2)}},
- "potentiometer_offset": 0.0
- },
+ {% set _ = intake_pivot_zero.update(
+ {
+ "measured_absolute_position" : 0.0
+ }
+ ) %}
+ "intake_constants": {{ intake_pivot_zero | tojson(indent=2)}},
"climber_constants": {
{% set _ = climber_zero.update(
{
diff --git a/y2024/constants/9971.json b/y2024/constants/9971.json
index 9e6bafb..d2e766a 100644
--- a/y2024/constants/9971.json
+++ b/y2024/constants/9971.json
@@ -3,15 +3,12 @@
{
"robot": {
- "intake_constants": {
- {% set _ = intake_pivot_zero.update(
- {
- "measured_absolute_position" : 0.0
- }
- ) %}
- "zeroing_constants": {{ intake_pivot_zero | tojson(indent=2)}},
- "potentiometer_offset": 0.0
- },
+ {% set _ = intake_pivot_zero.update(
+ {
+ "measured_absolute_position" : 0.0
+ }
+ ) %}
+ "intake_constants": {{ intake_pivot_zero | tojson(indent=2)}},
"climber_constants": {
{% set _ = climber_zero.update(
{
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json
new file mode 100755
index 0000000..63f4ea8
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-01_2024-02-07_20-11-35.566609408.json
@@ -0,0 +1,25 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 646.870789,
+ 0.0,
+ 731.468811,
+ 0.0,
+ 646.616333,
+ 570.003723,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.249077,
+ 0.063132,
+ 0.000082,
+ 0.00026,
+ -0.006916
+ ],
+ "calibration_timestamp": 1707365495566609408,
+ "camera_id": "24-01",
+ "camera_number": 0
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-04_2024-02-07_20-45-22.787382400.json b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-04_2024-02-07_20-45-22.787382400.json
new file mode 100755
index 0000000..a3ea964
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-0_cam-24-04_2024-02-07_20-45-22.787382400.json
@@ -0,0 +1,25 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 642.80365,
+ 0.0,
+ 718.017517,
+ 0.0,
+ 642.83667,
+ 555.022461,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.239969,
+ 0.055889,
+ 0.000086,
+ 0.000099,
+ -0.005468
+ ],
+ "calibration_timestamp": 1707367522787382400,
+ "camera_id": "24-04",
+ "camera_number": 0
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-02_2024-02-07_20-11-32.368359264.json b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-02_2024-02-07_20-11-32.368359264.json
new file mode 100755
index 0000000..482158c
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-02_2024-02-07_20-11-32.368359264.json
@@ -0,0 +1,25 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 644.604858,
+ 0.0,
+ 752.152954,
+ 0.0,
+ 644.477173,
+ 558.911682,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.242432,
+ 0.057303,
+ -0.000057,
+ 0.000015,
+ -0.005636
+ ],
+ "calibration_timestamp": 1707365492368359264,
+ "camera_id": "24-02",
+ "camera_number": 1
+}
\ No newline at end of file
diff --git a/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-03_2024-02-07_20-40-34.928600992.json b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-03_2024-02-07_20-40-34.928600992.json
new file mode 100755
index 0000000..febd9ef
--- /dev/null
+++ b/y2024/constants/calib_files/calibration_orin-971-1-1_cam-24-03_2024-02-07_20-40-34.928600992.json
@@ -0,0 +1,25 @@
+{
+ "node_name": "orin1",
+ "team_number": 971,
+ "intrinsics": [
+ 648.13446,
+ 0.0,
+ 759.733093,
+ 0.0,
+ 648.40332,
+ 557.951538,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.258663,
+ 0.071646,
+ 0.000113,
+ -0.000061,
+ -0.00879
+ ],
+ "calibration_timestamp": 1707367234928600992,
+ "camera_id": "24-03",
+ "camera_number": 1
+}
\ No newline at end of file
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 554f1e8..3fc95fe 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -5,7 +5,9 @@
"distance_from_goal": 0.0,
"shot_params": {
"shot_velocity": 0.0,
- "shot_angle": 0.0
+ "shot_altitude_angle": 0.0,
+ "shot_catapult_angle": 0.0,
+ "shot_speed_over_ground": 0.0
}
}
],
@@ -78,5 +80,7 @@
"upper": 2.0
},
"loop": {% include 'y2024/control_loops/superstructure/climber/integral_climber_plant.json' %}
- }
+ },
+ "turret_loading_position": 0.0,
+ "catapult_return_position": 0.0
}
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index 40d609f..f7f9e36 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -12,7 +12,15 @@
table ShotParams {
shot_velocity: double (id: 0);
- shot_angle: double (id: 1);
+
+ // Angle of the altitude
+ shot_altitude_angle: double (id: 1);
+
+ // Angle of the catapult
+ shot_catapult_angle: double (id: 2);
+
+ // Speed over ground to use for shooting on the fly
+ shot_speed_over_ground: double (id: 3);
}
table InterpolationTablePoint {
@@ -71,9 +79,8 @@
}
table RobotConstants {
- intake_constants:PotAndAbsEncoderConstants (id: 0);
+ intake_constants:frc971.zeroing.AbsoluteEncoderZeroingConstants (id: 0);
climber_constants:PotAndAbsEncoderConstants (id: 1);
-
}
// Common table for constants unrelated to the robot
@@ -88,6 +95,8 @@
transfer_roller_voltages:TransferRollerVoltages (id: 7);
climber:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 8);
climber_set_points:ClimberSetPoints (id: 9);
+ turret_loading_position: double (id: 10);
+ catapult_return_position: double (id: 11);
}
table Constants {
diff --git a/y2024/constants/test_data/test_team.json b/y2024/constants/test_data/test_team.json
index 9e6bafb..d2e766a 100644
--- a/y2024/constants/test_data/test_team.json
+++ b/y2024/constants/test_data/test_team.json
@@ -3,15 +3,12 @@
{
"robot": {
- "intake_constants": {
- {% set _ = intake_pivot_zero.update(
- {
- "measured_absolute_position" : 0.0
- }
- ) %}
- "zeroing_constants": {{ intake_pivot_zero | tojson(indent=2)}},
- "potentiometer_offset": 0.0
- },
+ {% set _ = intake_pivot_zero.update(
+ {
+ "measured_absolute_position" : 0.0
+ }
+ ) %}
+ "intake_constants": {{ intake_pivot_zero | tojson(indent=2)}},
"climber_constants": {
{% set _ = climber_zero.update(
{
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 18db6d6..1d7c5b5 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -31,9 +31,8 @@
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
transfer_goal_(TransferRollerGoal::NONE),
- intake_pivot_(
- robot_constants_->common()->intake_pivot(),
- robot_constants_->robot()->intake_constants()->zeroing_constants()),
+ intake_pivot_(robot_constants_->common()->intake_pivot(),
+ robot_constants_->robot()->intake_constants()),
climber_(
robot_constants_->common()->climber(),
robot_constants_->robot()->climber_constants()->zeroing_constants()) {
@@ -153,7 +152,7 @@
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*intake_pivot_goal = &intake_pivot_goal_buffer.message();
- const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ const flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus>
intake_pivot_status_offset = intake_pivot_.Iterate(
intake_pivot_goal, position->intake_pivot(),
output != nullptr ? &output_struct.intake_pivot_voltage : nullptr,
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 88db2e2..f85e0fc 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -21,6 +21,11 @@
class Superstructure
: public ::frc971::controls::ControlLoop<Goal, Position, Status, Output> {
public:
+ using AbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::AbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::AbsoluteEncoderProfiledJointStatus>;
+
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
@@ -30,7 +35,7 @@
std::shared_ptr<const constants::Values> values,
const ::std::string &name = "/superstructure");
- inline const PotAndAbsoluteEncoderSubsystem &intake_pivot() const {
+ inline const AbsoluteEncoderSubsystem &intake_pivot() const {
return intake_pivot_;
}
@@ -56,8 +61,9 @@
aos::Alliance alliance_ = aos::Alliance::kInvalid;
TransferRollerGoal transfer_goal_;
- PotAndAbsoluteEncoderSubsystem intake_pivot_;
+ AbsoluteEncoderSubsystem intake_pivot_;
PotAndAbsoluteEncoderSubsystem climber_;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2024/control_loops/superstructure/superstructure_can_position.fbs b/y2024/control_loops/superstructure/superstructure_can_position.fbs
index 6cd3f1e..69bcdd0 100644
--- a/y2024/control_loops/superstructure/superstructure_can_position.fbs
+++ b/y2024/control_loops/superstructure/superstructure_can_position.fbs
@@ -23,6 +23,9 @@
// CAN Position of the climber falcon
climber:frc971.control_loops.CANTalonFX (id: 5);
+
+ // CAN Position of the retention roller falcon
+ retention_roller:frc971.control_loops.CANTalonFX (id: 6);
}
-root_type CANPosition;
\ No newline at end of file
+root_type CANPosition;
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index bb7b706..6ec954f 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -35,11 +35,25 @@
RETRACT = 2,
}
+table ShooterGoal {
+ catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 0);
+ fire: bool (id: 1);
+ // If true we ignore the other provided positions
+ auto_aim: bool (id: 2);
+
+ // Position for the turret when we aren't auto aiming
+ turret_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);
+
+ // Position for the altitude when we aren't auto aiming
+ altitude_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 4);
+}
+
table Goal {
intake_roller_goal:IntakeRollerGoal (id: 0);
intake_pivot_goal:IntakePivotGoal (id: 1);
catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 2);
transfer_roller_goal:TransferRollerGoal (id: 3);
climber_goal:ClimberGoal (id: 4);
+ shooter_goal:ShooterGoal (id: 5);
}
root_type Goal;
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 8020a4d..4b4ac8d 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -33,11 +33,16 @@
using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
typedef Superstructure::PotAndAbsoluteEncoderSubsystem
PotAndAbsoluteEncoderSubsystem;
+typedef Superstructure::AbsoluteEncoderSubsystem AbsoluteEncoderSubsystem;
using PotAndAbsoluteEncoderSimulator =
frc971::control_loops::SubsystemSimulator<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
PotAndAbsoluteEncoderSubsystem::State,
constants::Values::PotAndAbsEncoderConstants>;
+using AbsoluteEncoderSimulator = frc971::control_loops::SubsystemSimulator<
+ frc971::control_loops::AbsoluteEncoderProfiledJointStatus,
+ AbsoluteEncoderSubsystem::State,
+ constants::Values::AbsoluteEncoderConstants>;
class SuperstructureSimulation {
public:
@@ -57,21 +62,14 @@
new CappedTestPlant(intake_pivot::MakeIntakePivotPlant()),
PositionSensorSimulator(simulated_robot_constants->robot()
->intake_constants()
- ->zeroing_constants()
->one_revolution_distance()),
{.subsystem_params =
{simulated_robot_constants->common()->intake_pivot(),
- simulated_robot_constants->robot()
- ->intake_constants()
- ->zeroing_constants()},
- .potentiometer_offset = simulated_robot_constants->robot()
- ->intake_constants()
- ->potentiometer_offset()},
+ simulated_robot_constants->robot()->intake_constants()}},
frc971::constants::Range::FromFlatbuffer(
simulated_robot_constants->common()->intake_pivot()->range()),
simulated_robot_constants->robot()
->intake_constants()
- ->zeroing_constants()
->measured_absolute_position(),
dt_),
climber_(new CappedTestPlant(climber::MakeClimberPlant()),
@@ -127,9 +125,9 @@
::aos::Sender<Position>::Builder builder =
superstructure_position_sender_.MakeBuilder();
- frc971::PotAndAbsolutePosition::Builder intake_pivot_builder =
- builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
- flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_pivot_offset =
+ frc971::AbsolutePosition::Builder intake_pivot_builder =
+ builder.MakeBuilder<frc971::AbsolutePosition>();
+ flatbuffers::Offset<frc971::AbsolutePosition> intake_pivot_offset =
intake_pivot_.encoder()->GetSensorValues(&intake_pivot_builder);
frc971::PotAndAbsolutePosition::Builder climber_builder =
@@ -151,7 +149,7 @@
transfer_beambreak_ = triggered;
}
- PotAndAbsoluteEncoderSimulator *intake_pivot() { return &intake_pivot_; }
+ AbsoluteEncoderSimulator *intake_pivot() { return &intake_pivot_; }
PotAndAbsoluteEncoderSimulator *climber() { return &climber_; }
@@ -166,7 +164,7 @@
bool transfer_beambreak_;
- PotAndAbsoluteEncoderSimulator intake_pivot_;
+ AbsoluteEncoderSimulator intake_pivot_;
PotAndAbsoluteEncoderSimulator climber_;
bool first_ = true;
@@ -223,6 +221,8 @@
superstructure_status_fetcher_.Fetch();
superstructure_output_fetcher_.Fetch();
+ ASSERT_FALSE(superstructure_status_fetcher_->estopped());
+
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
<< ": No status";
@@ -389,7 +389,6 @@
// Give it a lot of time to get there.
RunFor(chrono::seconds(15));
-
VerifyNearGoal();
}
@@ -435,7 +434,7 @@
WaitUntilZeroed();
RunFor(chrono::seconds(2));
- EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ EXPECT_EQ(AbsoluteEncoderSubsystem::State::RUNNING,
superstructure_.intake_pivot().state());
EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
diff --git a/y2024/control_loops/superstructure/superstructure_output.fbs b/y2024/control_loops/superstructure/superstructure_output.fbs
index a429976..ddfc5ac 100644
--- a/y2024/control_loops/superstructure/superstructure_output.fbs
+++ b/y2024/control_loops/superstructure/superstructure_output.fbs
@@ -25,6 +25,10 @@
// Positive voltage is for climber up
// Negative voltage is for climber down
climber_voltage:double (id: 6);
+
+ // Voltage of the retention rollers
+ // Positive voltage will hold the game piece in the catapult.
+ retention_roller_voltage: double (id: 7);
}
root_type Output;
diff --git a/y2024/control_loops/superstructure/superstructure_position.fbs b/y2024/control_loops/superstructure/superstructure_position.fbs
index cd12da0..8d2b7ca 100644
--- a/y2024/control_loops/superstructure/superstructure_position.fbs
+++ b/y2024/control_loops/superstructure/superstructure_position.fbs
@@ -5,7 +5,7 @@
table Position {
// Values of the encoder and potentiometer at the intake pivot
- intake_pivot:frc971.PotAndAbsolutePosition (id: 0);
+ intake_pivot:frc971.AbsolutePosition (id: 0);
// Values of the encoder and potentiometer at the turret
turret:frc971.PotAndAbsolutePosition (id: 1);
@@ -22,6 +22,9 @@
// Values of the encoder and potentiometer at the climber.
// Zero is fully retracted, positive is extended upward.
climber:frc971.PotAndAbsolutePosition (id: 5);
+
+ // True if there is a game piece in the catapult
+ catapult_beam_break:bool (id: 6);
}
root_type Position;
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index cd727ac..3a4b443 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -10,6 +10,15 @@
INTAKING = 2,
}
+enum CatapultStatus: ubyte {
+ // Means we are waiting for a game piece
+ IDLE = 0,
+ // Means we have a game piece
+ LOADED = 1,
+ // Means we are firing a game piece
+ FIRING = 2,
+}
+
table ShooterStatus {
// Estimated angle and angular velocitiy of the turret.
turret_state:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 0);
@@ -19,6 +28,8 @@
// Estimated angle and angular velocitiy of the altitude.
altitude_state:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);
+
+ catapult_status: CatapultStatus (id: 3);
}
// Contains status of transfer rollers
@@ -39,7 +50,7 @@
intake_roller_state:IntakeRollerState (id: 2);
// Estimated angle and angular velocitiy of the intake.
- intake_pivot_state:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 3);
+ intake_pivot_state:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
// State of transfer rollers
transfer_roller_state:TransferRollerState (id: 4);
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index 4a6c00b..b396426 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -77,10 +77,6 @@
constexpr double kMaxBringupPower = 12.0;
-double intake_pot_translate(double voltage) {
- return voltage * Values::kIntakePivotPotRadiansPerVolt();
-}
-
double climber_pot_translate(double voltage) {
return voltage * Values::kClimberPotRadiansPerVolt();
}
@@ -140,11 +136,7 @@
CopyPosition(intake_pivot_encoder_, builder->add_intake_pivot(),
Values::kIntakePivotEncoderCountsPerRevolution(),
- Values::kIntakePivotEncoderRatio(), intake_pot_translate,
- true,
- robot_constants_->robot()
- ->intake_constants()
- ->potentiometer_offset());
+ Values::kIntakePivotEncoderRatio(), /* reversed: */ false);
CopyPosition(climber_encoder_, builder->add_climber(),
Values::kClimberEncoderCountsPerRevolution(),
@@ -212,12 +204,10 @@
}
void set_intake_pivot(::std::unique_ptr<frc::Encoder> encoder,
- ::std::unique_ptr<frc::DigitalInput> absolute_pwm,
- ::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
fast_encoder_filter_.Add(encoder.get());
intake_pivot_encoder_.set_encoder(::std::move(encoder));
intake_pivot_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
- intake_pivot_encoder_.set_potentiometer(::std::move(potentiometer));
}
void set_transfer_beambreak(::std::unique_ptr<frc::DigitalInput> sensor) {
@@ -246,7 +236,7 @@
std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_, transfer_beam_break_;
- frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_pivot_encoder_;
+ frc971::wpilib::AbsoluteEncoder intake_pivot_encoder_;
frc971::wpilib::AbsoluteEncoderAndPotentiometer climber_encoder_;
frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
@@ -290,8 +280,7 @@
sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(0));
// TODO: (niko) change values once robot is wired
sensor_reader.set_intake_pivot(make_encoder(4),
- make_unique<frc::DigitalInput>(4),
- make_unique<frc::AnalogInput>(4));
+ make_unique<frc::DigitalInput>(4));
sensor_reader.set_transfer_beambreak(make_unique<frc::DigitalInput>(7));
sensor_reader.set_climber(make_encoder(5),
@@ -307,42 +296,42 @@
c_Phoenix_Diagnostics_Dispose();
}
- std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
+ std::vector<ctre::phoenix6::BaseStatusSignal *> canivore_signal_registry;
+ std::vector<ctre::phoenix6::BaseStatusSignal *> rio_signal_registry;
const CurrentLimits *current_limits =
robot_constants->common()->current_limits();
std::shared_ptr<TalonFX> right_front = std::make_shared<TalonFX>(
- 0, false, "Drivetrain Bus", &signals_registry,
+ 0, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->drivetrain_supply_current_limit(),
current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> right_back = std::make_shared<TalonFX>(
- 1, false, "Drivetrain Bus", &signals_registry,
+ 1, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->drivetrain_supply_current_limit(),
current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> left_front = std::make_shared<TalonFX>(
- 2, false, "Drivetrain Bus", &signals_registry,
+ 2, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->drivetrain_supply_current_limit(),
current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> left_back = std::make_shared<TalonFX>(
- 3, false, "Drivetrain Bus", &signals_registry,
+ 3, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->drivetrain_supply_current_limit(),
current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> intake_pivot = std::make_shared<TalonFX>(
- 4, false, "Drivetrain Bus", &signals_registry,
+ 4, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->intake_pivot_stator_current_limit(),
current_limits->intake_pivot_supply_current_limit());
std::shared_ptr<TalonFX> intake_roller = std::make_shared<TalonFX>(
- 5, false, "Drivetrain Bus", &signals_registry,
+ 5, false, "rio", &rio_signal_registry,
current_limits->intake_roller_stator_current_limit(),
current_limits->intake_roller_supply_current_limit());
std::shared_ptr<TalonFX> transfer_roller = std::make_shared<TalonFX>(
- 6, false, "Drivetrain Bus", &signals_registry,
+ 6, false, "rio", &rio_signal_registry,
current_limits->transfer_roller_stator_current_limit(),
current_limits->transfer_roller_supply_current_limit());
-
std::shared_ptr<TalonFX> climber = std::make_shared<TalonFX>(
- 7, false, "Drivetrain Bus", &signals_registry,
+ 7, false, "rio", &rio_signal_registry,
current_limits->climber_stator_current_limit(),
current_limits->climber_supply_current_limit());
@@ -354,18 +343,25 @@
::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
can_sensor_reader_event_loop.set_name("CANSensorReader");
+ ::aos::ShmEventLoop rio_sensor_reader_event_loop(&config.message());
+ rio_sensor_reader_event_loop.set_name("RioSensorReader");
+
// Creating list of talonfx for CANSensorReader
std::vector<std::shared_ptr<TalonFX>> drivetrain_talonfxs;
- std::vector<std::shared_ptr<TalonFX>> talonfxs;
+ std::vector<std::shared_ptr<TalonFX>> canivore_talonfxs;
+ std::vector<std::shared_ptr<TalonFX>> rio_talonfxs;
for (auto talonfx : {right_front, right_back, left_front, left_back}) {
drivetrain_talonfxs.push_back(talonfx);
- talonfxs.push_back(talonfx);
+ canivore_talonfxs.push_back(talonfx);
}
- for (auto talonfx :
- {intake_pivot, intake_roller, transfer_roller, climber}) {
- talonfxs.push_back(talonfx);
+ for (auto talonfx : {intake_pivot}) {
+ canivore_talonfxs.push_back(talonfx);
+ }
+
+ for (auto talonfx : {intake_roller, transfer_roller, climber}) {
+ rio_talonfxs.push_back(talonfx);
}
aos::Sender<frc971::control_loops::drivetrain::CANPositionStatic>
@@ -378,12 +374,12 @@
superstructure_can_position_sender =
can_sensor_reader_event_loop.MakeSender<
y2024::control_loops::superstructure::CANPositionStatic>(
- "/superstructure");
+ "/superstructure/canivore");
- frc971::wpilib::CANSensorReader can_sensor_reader(
- &can_sensor_reader_event_loop, std::move(signals_registry), talonfxs,
- [drivetrain_talonfxs, &intake_pivot, &intake_roller, &transfer_roller,
- &climber, &drivetrain_can_position_sender,
+ frc971::wpilib::CANSensorReader canivore_can_sensor_reader(
+ &can_sensor_reader_event_loop, std::move(canivore_signal_registry),
+ canivore_talonfxs,
+ [drivetrain_talonfxs, &intake_pivot, &drivetrain_can_position_sender,
&superstructure_can_position_sender](
ctre::phoenix::StatusCode status) {
aos::Sender<frc971::control_loops::drivetrain::CANPositionStatic>::
@@ -411,12 +407,35 @@
StaticBuilder superstructure_can_builder =
superstructure_can_position_sender.MakeStaticBuilder();
- intake_roller->SerializePosition(
- superstructure_can_builder->add_intake_roller(),
- control_loops::drivetrain::kHighOutputRatio);
intake_pivot->SerializePosition(
superstructure_can_builder->add_intake_pivot(),
control_loops::drivetrain::kHighOutputRatio);
+
+ superstructure_can_builder->set_timestamp(
+ intake_pivot->GetTimestamp());
+ superstructure_can_builder->set_status(static_cast<int>(status));
+ superstructure_can_builder.CheckOk(superstructure_can_builder.Send());
+ });
+
+ aos::Sender<y2024::control_loops::superstructure::CANPositionStatic>
+ superstructure_rio_position_sender =
+ rio_sensor_reader_event_loop.MakeSender<
+ y2024::control_loops::superstructure::CANPositionStatic>(
+ "/superstructure/rio");
+
+ frc971::wpilib::CANSensorReader rio_can_sensor_reader(
+ &rio_sensor_reader_event_loop, std::move(rio_signal_registry),
+ rio_talonfxs,
+ [&intake_roller, &transfer_roller, &climber,
+ &superstructure_rio_position_sender](
+ ctre::phoenix::StatusCode status) {
+ aos::Sender<y2024::control_loops::superstructure::CANPositionStatic>::
+ StaticBuilder superstructure_can_builder =
+ superstructure_rio_position_sender.MakeStaticBuilder();
+
+ intake_roller->SerializePosition(
+ superstructure_can_builder->add_intake_roller(),
+ control_loops::drivetrain::kHighOutputRatio);
transfer_roller->SerializePosition(
superstructure_can_builder->add_transfer_roller(),
control_loops::drivetrain::kHighOutputRatio);
@@ -431,6 +450,7 @@
});
AddLoop(&can_sensor_reader_event_loop);
+ AddLoop(&rio_sensor_reader_event_loop);
// Thread 5.
::aos::ShmEventLoop can_output_event_loop(&config.message());
diff --git a/y2024/y2024_orin_template.json b/y2024/y2024_orin_template.json
index 2bb2f46..9b165fc 100644
--- a/y2024/y2024_orin_template.json
+++ b/y2024/y2024_orin_template.json
@@ -110,18 +110,7 @@
"max_size": 208
},
{
- "name": "/orin{{ NUM }}/camera1",
- "type": "frc971.vision.CameraImage",
- "source_node": "orin{{ NUM }}",
- "channel_storage_duration": 1000000000,
- "frequency": 65,
- "max_size": 4752384,
- "num_readers": 6,
- "read_method": "PIN",
- "num_senders": 18
- },
- {
- "name": "/orin{{ NUM }}/camera2",
+ "name": "/orin{{ NUM }}/camera0",
"type": "frc971.vision.CameraImage",
"source_node": "orin{{ NUM }}",
"channel_storage_duration": 1000000000,
@@ -133,6 +122,17 @@
},
{
"name": "/orin{{ NUM }}/camera1",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "orin{{ NUM }}",
+ "channel_storage_duration": 1000000000,
+ "frequency": 65,
+ "max_size": 4752384,
+ "num_readers": 6,
+ "read_method": "PIN",
+ "num_senders": 18
+ },
+ {
+ "name": "/orin{{ NUM }}/camera0",
"type": "foxglove.CompressedImage",
"source_node": "orin{{ NUM }}",
"channel_storage_duration": 1000000000,
@@ -140,7 +140,7 @@
"max_size": 622384
},
{
- "name": "/orin{{ NUM }}/camera2",
+ "name": "/orin{{ NUM }}/camera1",
"type": "foxglove.CompressedImage",
"source_node": "orin{{ NUM }}",
"channel_storage_duration": 1000000000,
@@ -148,6 +148,13 @@
"max_size": 622384
},
{
+ "name": "/orin{{ NUM }}/camera0",
+ "type": "foxglove.ImageAnnotations",
+ "source_node": "orin{{ NUM }}",
+ "frequency": 65,
+ "max_size": 50000
+ },
+ {
"name": "/orin{{ NUM }}/camera1",
"type": "foxglove.ImageAnnotations",
"source_node": "orin{{ NUM }}",
@@ -155,11 +162,27 @@
"max_size": 50000
},
{
- "name": "/orin{{ NUM }}/camera2",
- "type": "foxglove.ImageAnnotations",
+ "name": "/orin{{ NUM }}/camera0",
+ "type": "frc971.vision.TargetMap",
"source_node": "orin{{ NUM }}",
"frequency": 65,
- "max_size": 50000
+ "num_senders": 2,
+ "max_size": 1024,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 4,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "orin{{ NUM }}"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
},
{
"name": "/orin{{ NUM }}/camera1",
@@ -185,37 +208,14 @@
]
},
{
- "name": "/orin{{ NUM }}/camera2",
- "type": "frc971.vision.TargetMap",
- "source_node": "orin{{ NUM }}",
- "frequency": 65,
- "num_senders": 2,
- "max_size": 1024,
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "imu"
- ],
- "destination_nodes": [
- {
- "name": "imu",
- "priority": 4,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "orin{{ NUM }}"
- ],
- "time_to_live": 5000000
- }
- ]
- },
- {
- "name": "/orin{{ NUM }}/aos/remote_timestamps/imu/orin{{ NUM }}/camera1/frc971-vision-TargetMap",
+ "name": "/orin{{ NUM }}/aos/remote_timestamps/imu/orin{{ NUM }}/camera0/frc971-vision-TargetMap",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 80,
"source_node": "orin{{ NUM }}",
"max_size": 208
},
{
- "name": "/orin{{ NUM }}/aos/remote_timestamps/imu/orin{{ NUM }}/camera2/frc971-vision-TargetMap",
+ "name": "/orin{{ NUM }}/aos/remote_timestamps/imu/orin{{ NUM }}/camera1/frc971-vision-TargetMap",
"type": "aos.message_bridge.RemoteMessage",
"frequency": 80,
"source_node": "orin{{ NUM }}",
@@ -296,22 +296,22 @@
]
},
{
- "name": "foxglove_image_converter1",
+ "name": "foxglove_image_converter0",
"executable_name": "foxglove_image_converter",
"user": "pi",
"args": [
- "--channel", "/camera1"
+ "--channel", "/camera0"
],
"nodes": [
"orin{{ NUM }}"
]
},
{
- "name": "foxglove_image_converter2",
+ "name": "foxglove_image_converter1",
"executable_name": "foxglove_image_converter",
"user": "pi",
"args": [
- "--channel", "/camera2"
+ "--channel", "/camera1"
],
"nodes": [
"orin{{ NUM }}"
@@ -326,11 +326,24 @@
]
},
{
- "name": "argus_camera1",
+ "name": "argus_camera0",
"executable_name": "argus_camera",
"args": [
"--enable_ftrace",
"--camera=0",
+ "--channel=/camera0",
+ ],
+ "user": "pi",
+ "nodes": [
+ "orin{{ NUM }}"
+ ]
+ },
+ {
+ "name": "argus_camera1",
+ "executable_name": "argus_camera",
+ "args": [
+ "--enable_ftrace",
+ "--camera=1",
"--channel=/camera1",
],
"user": "pi",
@@ -339,12 +352,10 @@
]
},
{
- "name": "argus_camera2",
- "executable_name": "argus_camera",
+ "name": "apriltag_detector0",
+ "executable_name": "apriltag_detector",
"args": [
- "--enable_ftrace",
- "--camera=1",
- "--channel=/camera2",
+ "--channel=/camera0",
],
"user": "pi",
"nodes": [
@@ -361,17 +372,6 @@
"nodes": [
"orin{{ NUM }}"
]
- },
- {
- "name": "apriltag_detector2",
- "executable_name": "apriltag_detector",
- "args": [
- "--channel=/camera2",
- ],
- "user": "pi",
- "nodes": [
- "orin{{ NUM }}"
- ]
}
],
"maps": [
diff --git a/y2024/y2024_roborio.json b/y2024/y2024_roborio.json
index 93caacc..6af86f0 100644
--- a/y2024/y2024_roborio.json
+++ b/y2024/y2024_roborio.json
@@ -139,6 +139,22 @@
"max_size": 448
},
{
+ "name": "/superstructure/canivore",
+ "type": "y2024.control_loops.superstructure.CANPosition",
+ "source_node": "roborio",
+ "frequency": 220,
+ "num_senders": 2,
+ "max_size": 400
+ },
+ {
+ "name": "/superstructure/rio",
+ "type": "y2024.control_loops.superstructure.CANPosition",
+ "source_node": "roborio",
+ "frequency": 220,
+ "num_senders": 2,
+ "max_size": 400
+ },
+ {
"name": "/can",
"type": "frc971.can_logger.CanFrame",
"source_node": "roborio",
