Merge "Exit Spline UI when window is closed"
diff --git a/README.md b/README.md
index d54d963..f81d423 100644
--- a/README.md
+++ b/README.md
@@ -26,7 +26,8 @@
1. Install the required packages:
```console
apt-get update
-apt-get install bazel python
+# TODO(james): Ideally, we shouldn't need to be installing libtinfo5...
+apt-get install bazel python libtinfo5
```
2. Allow Bazel's sandboxing to work:
Follow the direction in `doc/frc971.conf`.
diff --git a/WORKSPACE b/WORKSPACE
index 168e690..dced1f0 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -681,6 +681,7 @@
http_archive(
name = "halide_k8",
build_file = "@//debian:halide.BUILD",
+ sha256 = "c67185d50a99adba86f6b2cc43c7e2cf11bcdfba9052d05e764a89b456a50446",
strip_prefix = "halide/",
url = "http://www.frc971.org/Build-Dependencies/halide-linux-64-gcc53-800-65c26cba6a3eca2d08a0bccf113ca28746012cc3.tgz",
)
@@ -691,6 +692,7 @@
http_archive(
name = "halide_armhf",
build_file = "@//debian:halide.BUILD",
+ sha256 = "10564c559c9e04a173823413916d05fadd6e697d91bab21ddc5041190fa8f0f0",
strip_prefix = "halide/",
url = "http://www.frc971.org/Build-Dependencies/halide-arm32-linux-32-trunk-65c26cba6a3eca2d08a0bccf113ca28746012cc3.tgz",
)
diff --git a/aos/BUILD b/aos/BUILD
index da558a4..d35e74c 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -526,3 +526,17 @@
"@com_github_google_glog//:glog",
],
)
+
+cc_library(
+ name = "ftrace",
+ srcs = [
+ "ftrace.cc",
+ ],
+ hdrs = [
+ "ftrace.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/aos/events/BUILD b/aos/events/BUILD
index d35f206..39a6a54 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -74,6 +74,7 @@
"//aos:configuration",
"//aos:configuration_fbs",
"//aos:flatbuffers",
+ "//aos:ftrace",
"//aos/ipc_lib:data_alignment",
"//aos/logging:implementations",
"//aos/time",
diff --git a/aos/events/event_loop.cc b/aos/events/event_loop.cc
index c03425e..da89d9b 100644
--- a/aos/events/event_loop.cc
+++ b/aos/events/event_loop.cc
@@ -14,6 +14,7 @@
RawSender::RawSender(EventLoop *event_loop, const Channel *channel)
: event_loop_(event_loop),
channel_(channel),
+ ftrace_prefix_(configuration::StrippedChannelToString(channel)),
timing_(event_loop_->ChannelIndex(channel)) {
event_loop_->NewSender(this);
}
@@ -23,6 +24,7 @@
RawFetcher::RawFetcher(EventLoop *event_loop, const Channel *channel)
: event_loop_(event_loop),
channel_(channel),
+ ftrace_prefix_(configuration::StrippedChannelToString(channel)),
timing_(event_loop_->ChannelIndex(channel)) {
context_.monotonic_event_time = monotonic_clock::min_time;
context_.monotonic_remote_time = monotonic_clock::min_time;
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 7634479..804d6ef 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -14,6 +14,7 @@
#include "aos/events/event_loop_generated.h"
#include "aos/events/timing_statistics.h"
#include "aos/flatbuffers.h"
+#include "aos/ftrace.h"
#include "aos/ipc_lib/data_alignment.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/time/time.h"
@@ -92,10 +93,12 @@
virtual std::pair<bool, monotonic_clock::time_point> DoFetchNext() = 0;
virtual std::pair<bool, monotonic_clock::time_point> DoFetch() = 0;
- EventLoop *event_loop_;
- const Channel *channel_;
+ EventLoop *const event_loop_;
+ const Channel *const channel_;
+ const std::string ftrace_prefix_;
internal::RawFetcherTiming timing_;
+ Ftrace ftrace_;
};
// Raw version of sender. Sends a block of data. This is used for reflection
@@ -174,10 +177,12 @@
aos::realtime_clock::time_point realtime_remote_time,
uint32_t remote_queue_index) = 0;
- EventLoop *event_loop_;
- const Channel *channel_;
+ EventLoop *const event_loop_;
+ const Channel *const channel_;
+ const std::string ftrace_prefix_;
internal::RawSenderTiming timing_;
+ Ftrace ftrace_;
ChannelPreallocatedAllocator fbb_allocator_{nullptr, 0, nullptr};
};
@@ -358,6 +363,7 @@
std::string name_;
internal::TimerTiming timing_;
+ Ftrace ftrace_;
};
// Interface for phased loops. They are built on timers.
@@ -404,6 +410,7 @@
int cycles_elapsed_ = 0;
internal::TimerTiming timing_;
+ Ftrace ftrace_;
};
inline cpu_set_t MakeCpusetFromCpus(std::initializer_list<int> cpus) {
@@ -594,12 +601,18 @@
// watcher must exist before calling this.
WatcherState *GetWatcherState(const Channel *channel);
- // Returns a Sender's protected RawSender
+ // Returns a Sender's protected RawSender.
template <typename T>
static RawSender *GetRawSender(aos::Sender<T> *sender) {
return sender->sender_.get();
}
+ // Returns a Fetcher's protected RawFetcher.
+ template <typename T>
+ static RawFetcher *GetRawFetcher(aos::Fetcher<T> *fetcher) {
+ return fetcher->fetcher_.get();
+ }
+
// Context available for watchers, timers, and phased loops.
Context context_;
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
index 5534c83..877a946 100644
--- a/aos/events/event_loop_tmpl.h
+++ b/aos/events/event_loop_tmpl.h
@@ -1,7 +1,11 @@
#ifndef AOS_EVENTS_EVENT_LOOP_TMPL_H_
#define AOS_EVENTS_EVENT_LOOP_TMPL_H_
+#include <inttypes.h>
+#include <stdint.h>
+
#include <type_traits>
+
#include "aos/events/event_loop.h"
#include "glog/logging.h"
@@ -72,6 +76,13 @@
if (result.first) {
timing_.fetcher->mutate_count(timing_.fetcher->count() + 1);
const monotonic_clock::time_point monotonic_time = result.second;
+ ftrace_.FormatMessage(
+ "%.*s: fetch next: now=%" PRId64 " event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_time.time_since_epoch().count()),
+ static_cast<int64_t>(
+ context_.monotonic_event_time.time_since_epoch().count()),
+ context_.queue_index);
const float latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
monotonic_time - context_.monotonic_event_time)
@@ -79,6 +90,12 @@
timing_.latency.Add(latency);
return true;
}
+ ftrace_.FormatMessage(
+ "%.*s: fetch next: still event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(
+ context_.monotonic_event_time.time_since_epoch().count()),
+ context_.queue_index);
return false;
}
@@ -87,6 +104,13 @@
if (result.first) {
timing_.fetcher->mutate_count(timing_.fetcher->count() + 1);
const monotonic_clock::time_point monotonic_time = result.second;
+ ftrace_.FormatMessage(
+ "%.*s: fetch latest: now=%" PRId64 " event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_time.time_since_epoch().count()),
+ static_cast<int64_t>(
+ context_.monotonic_event_time.time_since_epoch().count()),
+ context_.queue_index);
const float latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
monotonic_time - context_.monotonic_event_time)
@@ -94,6 +118,12 @@
timing_.latency.Add(latency);
return true;
}
+ ftrace_.FormatMessage(
+ "%.*s: fetch latest: still event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(
+ context_.monotonic_event_time.time_since_epoch().count()),
+ context_.queue_index);
return false;
}
@@ -105,6 +135,11 @@
remote_queue_index)) {
timing_.size.Add(size);
timing_.sender->mutate_count(timing_.sender->count() + 1);
+ ftrace_.FormatMessage(
+ "%.*s: sent internal: event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_sent_time().time_since_epoch().count()),
+ sent_queue_index());
return true;
}
return false;
@@ -119,6 +154,11 @@
remote_queue_index)) {
timing_.size.Add(size);
timing_.sender->mutate_count(timing_.sender->count() + 1);
+ ftrace_.FormatMessage(
+ "%.*s: sent external: event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_sent_time().time_since_epoch().count()),
+ sent_queue_index());
return true;
}
return false;
@@ -138,6 +178,11 @@
event_loop_->context_.size = 0;
event_loop_->context_.data = nullptr;
+ ftrace_.FormatMessage(
+ "timer: %.*s: start now=%" PRId64 " event=%" PRId64,
+ static_cast<int>(name_.size()), name_.data(),
+ static_cast<int64_t>(monotonic_start_time.time_since_epoch().count()),
+ static_cast<int64_t>(event_time.time_since_epoch().count()));
{
const float start_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -149,6 +194,10 @@
fn_();
const monotonic_clock::time_point monotonic_end_time = get_time();
+ ftrace_.FormatMessage(
+ "timer: %.*s: end now=%" PRId64, static_cast<int>(name_.size()),
+ name_.data(),
+ static_cast<int64_t>(monotonic_end_time.time_since_epoch().count()));
const float handler_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -176,6 +225,13 @@
// Compute how many cycles elapsed and schedule the next wakeup.
Reschedule(schedule, monotonic_start_time);
+ ftrace_.FormatMessage(
+ "phased: %.*s: start now=%" PRId64 " event=%" PRId64 " cycles=%d",
+ static_cast<int>(name_.size()), name_.data(),
+ static_cast<int64_t>(monotonic_start_time.time_since_epoch().count()),
+ static_cast<int64_t>(
+ phased_loop_.sleep_time().time_since_epoch().count()),
+ cycles_elapsed_);
{
const float start_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -190,6 +246,10 @@
cycles_elapsed_ = 0;
const monotonic_clock::time_point monotonic_end_time = get_time();
+ ftrace_.FormatMessage(
+ "phased: %.*s: end now=%" PRId64, static_cast<int>(name_.size()),
+ name_.data(),
+ static_cast<int64_t>(monotonic_end_time.time_since_epoch().count()));
const float handler_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -210,7 +270,9 @@
WatcherState(
EventLoop *event_loop, const Channel *channel,
std::function<void(const Context &context, const void *message)> fn)
- : channel_index_(event_loop->ChannelIndex(channel)), fn_(std::move(fn)) {}
+ : channel_index_(event_loop->ChannelIndex(channel)),
+ ftrace_prefix_(configuration::StrippedChannelToString(channel)),
+ fn_(std::move(fn)) {}
virtual ~WatcherState() {}
@@ -222,6 +284,13 @@
CheckChannelDataAlignment(context.data, context.size);
}
const monotonic_clock::time_point monotonic_start_time = get_time();
+ ftrace_.FormatMessage(
+ "%.*s: watcher start: now=%" PRId64 " event=%" PRId64 " queue=%" PRIu32,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_start_time.time_since_epoch().count()),
+ static_cast<int64_t>(
+ context.monotonic_event_time.time_since_epoch().count()),
+ context.queue_index);
{
const float start_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -233,6 +302,10 @@
fn_(context, context.data);
const monotonic_clock::time_point monotonic_end_time = get_time();
+ ftrace_.FormatMessage(
+ "%.*s: watcher end: now=%" PRId64,
+ static_cast<int>(ftrace_prefix_.size()), ftrace_prefix_.data(),
+ static_cast<int64_t>(monotonic_end_time.time_since_epoch().count()));
const float handler_latency =
std::chrono::duration_cast<std::chrono::duration<float>>(
@@ -250,12 +323,15 @@
protected:
const int channel_index_;
+ const std::string ftrace_prefix_;
- std::function<void(const Context &context, const void *message)> fn_;
+ const std::function<void(const Context &context, const void *message)> fn_;
internal::TimingStatistic wakeup_latency_;
internal::TimingStatistic handler_time_;
timing::Watcher *watcher_ = nullptr;
+
+ Ftrace ftrace_;
};
template <typename T>
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index ebfcc07..1315510 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -316,6 +316,13 @@
return lockless_queue_memory_.GetSharedMemory();
}
+ absl::Span<char> GetPrivateMemory() const {
+ CHECK(copy_data());
+ return absl::Span<char>(
+ const_cast<SimpleShmFetcher *>(this)->data_storage_start(),
+ lockless_queue_.message_data_size());
+ }
+
private:
char *data_storage_start() {
if (!copy_data()) return nullptr;
@@ -360,6 +367,10 @@
return std::make_pair(false, monotonic_clock::min_time);
}
+ absl::Span<char> GetPrivateMemory() const {
+ return simple_shm_fetcher_.GetPrivateMemory();
+ }
+
private:
SimpleShmFetcher simple_shm_fetcher_;
};
@@ -920,6 +931,11 @@
return static_cast<const ShmSender *>(sender)->GetSharedMemory();
}
+absl::Span<char> ShmEventLoop::GetShmFetcherPrivateMemory(
+ const aos::RawFetcher *fetcher) const {
+ return static_cast<const ShmFetcher *>(fetcher)->GetPrivateMemory();
+}
+
pid_t ShmEventLoop::GetTid() { return syscall(SYS_gettid); }
} // namespace aos
diff --git a/aos/events/shm_event_loop.h b/aos/events/shm_event_loop.h
index f832242..55fc85f 100644
--- a/aos/events/shm_event_loop.h
+++ b/aos/events/shm_event_loop.h
@@ -81,12 +81,19 @@
// this.
absl::Span<char> GetWatcherSharedMemory(const Channel *channel);
- // Returns the local mapping of the shared memory used by the provided Sender
+ // Returns the local mapping of the shared memory used by the provided Sender.
template <typename T>
absl::Span<char> GetSenderSharedMemory(aos::Sender<T> *sender) const {
return GetShmSenderSharedMemory(GetRawSender(sender));
}
+ // Returns the local mapping of the private memory used by the provided
+ // Fetcher to hold messages.
+ template <typename T>
+ absl::Span<char> GetFetcherPrivateMemory(aos::Fetcher<T> *fetcher) const {
+ return GetShmFetcherPrivateMemory(GetRawFetcher(fetcher));
+ }
+
private:
friend class shm_event_loop_internal::ShmWatcherState;
friend class shm_event_loop_internal::ShmTimerHandler;
@@ -107,9 +114,13 @@
// Returns the TID of the event loop.
pid_t GetTid() override;
- // Private method to access the shared memory mapping of a ShmSender
+ // Private method to access the shared memory mapping of a ShmSender.
absl::Span<char> GetShmSenderSharedMemory(const aos::RawSender *sender) const;
+ // Private method to access the private memory mapping of a ShmFetcher.
+ absl::Span<char> GetShmFetcherPrivateMemory(
+ const aos::RawFetcher *fetcher) const;
+
std::vector<std::function<void()>> on_run_;
int priority_ = 0;
cpu_set_t affinity_ = DefaultAffinity();
diff --git a/aos/events/shm_event_loop_test.cc b/aos/events/shm_event_loop_test.cc
index 0c39704..3244c39 100644
--- a/aos/events/shm_event_loop_test.cc
+++ b/aos/events/shm_event_loop_test.cc
@@ -207,11 +207,21 @@
auto generic_loop1 = factory.MakePrimary("primary");
ShmEventLoop *const loop1 = static_cast<ShmEventLoop *>(generic_loop1.get());
- // check that GetSenderSharedMemory returns non-null/non-empty memory span
+ // check that GetSenderSharedMemory returns non-null/non-empty memory span.
auto sender = loop1->MakeSender<TestMessage>("/test");
EXPECT_FALSE(loop1->GetSenderSharedMemory(&sender).empty());
}
+TEST(ShmEventLoopTest, GetFetcherPrivateMemory) {
+ ShmEventLoopTestFactory factory;
+ auto generic_loop1 = factory.MakePrimary("primary");
+ ShmEventLoop *const loop1 = static_cast<ShmEventLoop *>(generic_loop1.get());
+
+ // check that GetFetcherPrivateMemory returns non-null/non-empty memory span.
+ auto fetcher = loop1->MakeFetcher<TestMessage>("/test");
+ EXPECT_FALSE(loop1->GetFetcherPrivateMemory(&fetcher).empty());
+}
+
// TODO(austin): Test that missing a deadline with a timer recovers as expected.
} // namespace testing
diff --git a/aos/events/simulated_event_loop.h b/aos/events/simulated_event_loop.h
index 7718cfb..c8029de 100644
--- a/aos/events/simulated_event_loop.h
+++ b/aos/events/simulated_event_loop.h
@@ -2,6 +2,7 @@
#define AOS_EVENTS_SIMULATED_EVENT_LOOP_H_
#include <algorithm>
+#include <functional>
#include <map>
#include <memory>
#include <string_view>
diff --git a/aos/flatbuffer_introspection.cc b/aos/flatbuffer_introspection.cc
index 6f32424..ae402fe 100644
--- a/aos/flatbuffer_introspection.cc
+++ b/aos/flatbuffer_introspection.cc
@@ -351,6 +351,14 @@
std::string FlatbufferToJson(const reflection::Schema *schema,
const uint8_t *data, bool multi_line,
size_t max_vector_size) {
+ CHECK(schema != nullptr) << ": Need to provide a schema";
+
+ // It is pretty common to get passed in a nullptr when a test fails. Rather
+ // than CHECK, return a more user friendly result.
+ if (data == nullptr) {
+ return "null";
+ }
+
const flatbuffers::Table *table = flatbuffers::GetAnyRoot(data);
const reflection::Object *obj = schema->root_table();
diff --git a/aos/flatbuffer_introspection_test.cc b/aos/flatbuffer_introspection_test.cc
index 4214202..9dacd41 100644
--- a/aos/flatbuffer_introspection_test.cc
+++ b/aos/flatbuffer_introspection_test.cc
@@ -464,5 +464,20 @@
"}");
}
+// Tests that a nullptr buffer prints nullptr.
+TEST_F(FlatbufferIntrospectionTest, NullptrData) {
+ EXPECT_EQ("null", FlatbufferToJson(schema_, nullptr));
+}
+
+// Tests that a null schema gets caught.
+TEST(FlatbufferIntrospectionDeathTest, NullSchema) {
+ EXPECT_DEATH(
+ {
+ FlatbufferToJson(static_cast<const reflection::Schema *>(nullptr),
+ nullptr);
+ },
+ "Need to provide a schema");
+}
+
} // namespace testing
} // namespace aos
diff --git a/aos/ftrace.cc b/aos/ftrace.cc
new file mode 100644
index 0000000..af4964c
--- /dev/null
+++ b/aos/ftrace.cc
@@ -0,0 +1,45 @@
+#include "aos/ftrace.h"
+
+#include <stdarg.h>
+#include <stdio.h>
+
+namespace aos {
+
+Ftrace::~Ftrace() {
+ if (message_fd_ != -1) {
+ PCHECK(close(message_fd_) == 0);
+ }
+ if (message_fd_ != -1) {
+ PCHECK(close(on_fd_) == 0);
+ }
+}
+
+void Ftrace::FormatMessage(const char *format, ...) {
+ if (message_fd_ == -1) {
+ return;
+ }
+ char buffer[512];
+ va_list ap;
+ va_start(ap, format);
+ const int result = vsnprintf(buffer, sizeof(buffer), format, ap);
+ va_end(ap);
+ CHECK_LE(static_cast<size_t>(result), sizeof(buffer))
+ << ": Format string ended up too long: " << format;
+ WriteMessage(std::string_view(buffer, result));
+}
+
+void Ftrace::WriteMessage(std::string_view content) {
+ if (message_fd_ == -1) {
+ return;
+ }
+ const int result = write(message_fd_, content.data(), content.size());
+ if (result == -1 && errno == EBADF) {
+ // This just means tracing is turned off. Ignore it.
+ return;
+ }
+ PCHECK(result >= 0) << ": Failed to write ftrace message: " << content;
+ CHECK_EQ(static_cast<size_t>(result), content.size())
+ << ": Failed to write complete ftrace message: " << content;
+}
+
+} // namespace aos
diff --git a/aos/ftrace.h b/aos/ftrace.h
new file mode 100644
index 0000000..42147ad
--- /dev/null
+++ b/aos/ftrace.h
@@ -0,0 +1,51 @@
+#ifndef AOS_FTRACE_H_
+#define AOS_FTRACE_H_
+
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <string_view>
+
+#include "glog/logging.h"
+
+namespace aos {
+
+// Manages interacting with ftrace. Silently hides many errors, because they are
+// expected to occur when ftrace is not enabled, and we want calling code to
+// continue working in that case.
+class Ftrace {
+ public:
+ Ftrace()
+ : message_fd_(open("/sys/kernel/debug/tracing/trace_marker", O_WRONLY)),
+ on_fd_(open("/sys/kernel/debug/tracing/tracing_on", O_WRONLY)) {}
+ ~Ftrace();
+
+ // Writes a message with a printf-style format.
+ //
+ // Silently does nothing if tracing is disabled.
+ void FormatMessage(const char *format, ...)
+ __attribute__((format(__printf__, 2, 3)));
+
+ // Writes a preformatted message.
+ //
+ // Silently does nothing if tracing is disabled.
+ void WriteMessage(std::string_view content);
+
+ // Turns tracing off, or CHECK-fails if tracing is inaccessible. Does nothing
+ // if tracing is currently available but off.
+ void TurnOffOrDie() {
+ CHECK(on_fd_ != -1)
+ << ": Failed to open tracing_on earlier, cannot turn off tracing";
+ char zero = '0';
+ CHECK_EQ(write(on_fd_, &zero, 1), 1) << ": Failed to turn tracing off";
+ }
+
+ private:
+ int message_fd_, on_fd_;
+};
+
+} // namespace aos
+
+#endif // AOS_FTRACE_H_
diff --git a/aos/json_to_flatbuffer_test.cc b/aos/json_to_flatbuffer_test.cc
index a9112f4..6dbb7ea 100644
--- a/aos/json_to_flatbuffer_test.cc
+++ b/aos/json_to_flatbuffer_test.cc
@@ -231,5 +231,11 @@
EXPECT_EQ("{ \"vector_foo_int\": [ ... 101 elements ... ] }", back_json_long);
}
+// Tests that a nullptr buffer prints nullptr.
+TEST_F(JsonToFlatbufferTest, NullptrData) {
+ EXPECT_EQ("null", TableFlatbufferToJson((const flatbuffers::Table *)(nullptr),
+ ConfigurationTypeTable(), false));
+}
+
} // namespace testing
} // namespace aos
diff --git a/documentation/tutorials/BUILD b/documentation/tutorials/BUILD
index 13516c1..5816444 100644
--- a/documentation/tutorials/BUILD
+++ b/documentation/tutorials/BUILD
@@ -6,10 +6,8 @@
"create-a-simple-program-for-running-a-motor",
"download-code-to-the-robot",
"make-a-drivebase-move",
- "send-and-receive-messages-on-queues",
"submitting-code-for-a-review",
"tune-an-autonomous",
- "generated-queue-code",
"using-bazel",
]
diff --git a/documentation/tutorials/generated-queue-code.md b/documentation/tutorials/generated-queue-code.md
deleted file mode 100644
index 8f00a31..0000000
--- a/documentation/tutorials/generated-queue-code.md
+++ /dev/null
@@ -1,246 +0,0 @@
-# Generated queue code
-
-This tutorial covers what the code that is generated from a .q file looks like.
-
-### The basics
-Each `.q` file generates a `.cc` and `.h` pair that can be #include'd in your c++
-code.
-
-We're going to use `//frc971/queues:gyro.q` as our example.
-
-## Package declaration
-`.q` files usually start with a `package` declaration. The `package` declaration
-directly converts to a set of namespace declarations. So
-
-```cpp
-package frc971.sensors;
-```
-
-generates the namespace definitions
-
-```cpp
-namespace frc971 {
-namespace sensors {
-
-// All of the code for the queue
-
-} // namespace sensors
-} // namespace frc971
-```
-
-## Message declarations
-Each message declared in the queue generates a class that can be instantiated in
-your code easily.
-
-For a simple example, lets use the `Uid` message in `//frc971/queues:gyro.q`:
-
-```cpp
-message Uid {
- uint32_t uid;
-};
-```
-
-Let's take a look at the actual class definition that is created. If you ever
-want to inspect the code that is generated from a .q file, you can take a look
-in `bazel-genfiles` after you build your code. So after running `bazel build
-//frc971/queues:gyro` there will be a `bazel-genfiles/frc971/queues/gyro.q.h`
-and `bazel-genfiles/frc971/queues/gyro.q.cc`.
-
-Here's the definition of the `Uid` class from
-`bazel-genfiles/frc971/queues/gyro.q.h` (comments mine):
-
-```cpp
-struct Uid : public ::aos::Message {
- // Used to identify queues uniquely even if they have the same name.
- enum { kQueueLength = 100, kHash = 0x0837c541 };
-
- // The actual data that we requested be part of the message.
- uint32_t uid;
-
- // Writes the message to a byte buffer.
- size_t Serialize(char *buffer) const;
- // Reads the message from a byte buffer.
- size_t Deserialize(const char *buffer);
- // Zeroes all of the fields in the message.
- void Zero();
- // Returns the size of message. Not used
- static size_t Size() { return 4 + ::aos::Message::Size(); }
- // Prints the contents of the message to a string that is human readable.
- size_t Print(char *buffer, size_t length) const;
-
- // Gets information about this message's type.
- static const ::aos::MessageType *GetType();
- static const ::aos::MessageType *DoGetType();
-
- // Default constructor, zeroes the struct.
- Uid();
- // Value constructor, sets the data at construction.
- Uid(uint32_t uid_in);
- bool EqualsNoTime(const Uid &other) const;
-};
-```
-
-## Queue declarations
-Every `queue` declaration in the `.q` file creates an `aos::Queue` variable that
-uses the message type given. So the declaration
-
-```cpp
-queue Uid gyro_part_id;
-```
-
-will generate a variable in `gyro.q.h`
-
-```cpp
-static UNUSED_VARIABLE::aos::Queue<Uid> &gyro_part_id;
-```
-
-which can then be used in any file that includes the header file.
-
-
-## QueueGroup declarations
-
-`queue_group` declares a set of queues and some associated helpers for them.
-There are two kinds of `queue_groups`, declarations and aliases. Here's an
-example of a `queue_group` definition:
-
-```cpp
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- IntakeGoal intake;
-
- // Used to identify a position in the planned set of positions on the arm.
- uint32_t arm_goal_position;
- // If true, start the grab box sequence.
- bool grab_box;
-
- bool open_claw;
- bool close_claw;
-
- bool deploy_fork;
-
- bool hook_release;
-
- double voltage_winch;
-
- double open_threshold;
-
- bool disable_box_correct;
-
- bool trajectory_override;
- };
-
- message Status {
- // Are all the subsystems zeroed?
- bool zeroed;
-
- // If true, any of the subsystems have aborted.
- bool estopped;
-
- // Status of both intake sides.
- IntakeSideStatus left_intake;
- IntakeSideStatus right_intake;
-
- ArmStatus arm;
-
- double filtered_box_velocity;
- uint32_t rotation_state;
- };
-
- message Position {
- // Values of the series elastic encoders on the left side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors left_intake;
-
- // Values of the series elastic encoders on the right side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors right_intake;
-
- ArmPosition arm;
-
- // Value of the beam breaker sensor. This value is true if the beam is
- // broken, false if the beam isn't broken.
- bool claw_beambreak_triggered;
- // Value of the beambreak sensor detecting when the box has hit the frame
- // cutout.
- bool box_back_beambreak_triggered;
-
- // Distance to the box in meters.
- double box_distance;
- };
-
- message Output {
- // Voltage sent to the parts on the left side of the intake.
- IntakeVoltage left_intake;
-
- // Voltage sent to the parts on the right side of the intake.
- IntakeVoltage right_intake;
-
- // Voltage sent to the motors on the proximal joint of the arm.
- double voltage_proximal;
-
- // Voltage sent to the motors on the distal joint of the arm.
- double voltage_distal;
-
- // Voltage sent to the hanger. Positive pulls the robot up.
- double voltage_winch;
-
- // Clamped (when true) or unclamped (when false) status sent to the
- // pneumatic claw on the arm.
- bool claw_grabbed;
-
- // If true, release the arm brakes.
- bool release_arm_brake;
- // If true, release the hook
- bool hook_release;
- // If true, release the forks
- bool forks_release;
- };
-
- queue Goal goal;
- queue Output output;
- queue Status status;
- queue Position position;
-};
-```
-
-and aliases look like:
-
-```cpp
-queue_group SuperstructureQueue superstructure_queue;
-```
-
-The declaration type creates the definition of the queue group and what messages
-and queues it contains. The alias type creates a variable with the given name.
-
-This queue group results in the following code in
-`bazel-genfiles/y2018/control_loops/superstructure/superstructure.q.h`
-
-```cpp
-
-struct SuperstructureQueue_Goal : public ::aos::Message { /* ... */ }
-struct SuperstructureQueue_Output : public ::aos::Message { /* ... */ }
-struct SuperstructureQueue_Status : public ::aos::Message { /* ... */ }
-struct SuperstructureQueue_Position : public ::aos::Message { /* ... */ }
-class SuperstructureQueue : public ::aos::QueueGroup {
- public:
- typedef SuperstructureQueue_Goal Goal;
- ::aos::Queue<SuperstructureQueue_Goal> goal;
- typedef SuperstructureQueue_Output Output;
- ::aos::Queue<SuperstructureQueue_Output> output;
- typedef SuperstructureQueue_Status Status;
- ::aos::Queue<SuperstructureQueue_Status> status;
- typedef SuperstructureQueue_Position Position;
- ::aos::Queue<SuperstructureQueue_Position> position;
- SuperstructureQueue(const char *name, uint32_t hash, const char *goal_name,
- const char *output_name, const char *status_name,
- const char *position_name);
-};
-```
-
-and a definition of a variable:
-
-```cpp
-static UNUSED_VARIABLE SuperstructureQueue &superstructure_queue;
-```
diff --git a/documentation/tutorials/send-and-receive-messages-on-queues.md b/documentation/tutorials/send-and-receive-messages-on-queues.md
deleted file mode 100644
index bcdaac1..0000000
--- a/documentation/tutorials/send-and-receive-messages-on-queues.md
+++ /dev/null
@@ -1,74 +0,0 @@
-# How to send and receive messages on a Queue
-
-Let's say we have a `//y2018:basic_queue.q` file that has the following
-contents:
-
-```cpp
-package y2018;
-
-message BasicMessage {
- float value;
-};
-
-queue BasicMessage basic_queue;
-```
-
-and the corresponding entry in `//y2018/BUILD`
-
-```python
-queue_library(
- name = "basic_queue",
- srcs = [
- "basic_queue.q",
- ],
- visibility = ["//visibility:public"],
-)
-```
-
-The following examples assume that you've declared a dependency on
-`//y2018:basic_queue` in the relevant `BUILD` file.
-
-## Sending a message
-First, include the queue definition that is created from the `.q` file. This
-will make the `basic_queue` variable available.
-
-```cpp
-#include "y2018/basic_queue.q.h"
-```
-
-```cpp
-
-// Allocate a new message pointer to use. basic_queue is from the include
-// statement.
-auto new_basic_message = basic_queue.MakeMessage();
-new_goal.value = 0.5;
-
-// Send the message on the queue
-if (!new_basic_message.Send()) {
- LOG(ERROR, "Failed to send.\n");
-}
-```
-
-## Receiving a message
-
-Once again, we must include the queue header file to get access to the queue.
-
-
-```cpp
-#include "y2018/basic_queue.q.h"
-```
-
-Then we can receive a message.
-
-
-```cpp
-basic_queue.FetchLatest();
-if (basic_queue.get()) {
- // We have a valid message ready to be used
- LOG(INFO, "Got a BasicMessage from basic_queue with value %f\n",
- basic_queue->value);
-} else {
- // No new message was available
- LOG(ERROR, "No message was received\n");
-}
-```
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index e03d581..bd358ca 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -138,6 +138,18 @@
static constexpr int kNInputs = 4;
// Number of previous samples to save.
static constexpr int kSaveSamples = 50;
+ // Whether we should completely rerun the entire stored history of
+ // kSaveSamples on every correction. Enabling this will increase overall CPU
+ // usage substantially; however, leaving it disabled makes it so that we are
+ // less likely to notice if processing camera frames is causing delays in the
+ // drivetrain.
+ // If we are having CPU issues, we have three easy avenues to improve things:
+ // (1) Reduce kSaveSamples (e.g., if all camera frames arive within
+ // 100 ms, then we can reduce kSaveSamples to be 25 (125 ms of samples)).
+ // (2) Don't actually rely on the ability to insert corrections into the
+ // timeline.
+ // (3) Set this to false.
+ static constexpr bool kFullRewindOnEverySample = false;
// Assume that all correction steps will have kNOutputs
// dimensions.
// TODO(james): Relax this assumption; relaxing it requires
@@ -182,19 +194,24 @@
X_hat_ = state;
have_zeroed_encoders_ = true;
P_ = P;
- observations_.PushFromBottom(
- {t,
- t,
- X_hat_,
- P_,
- Input::Zero(),
- Output::Zero(),
- {},
- [](const State &, const Input &) { return Output::Zero(); },
- [](const State &) {
- return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero();
- },
- Eigen::Matrix<Scalar, kNOutputs, kNOutputs>::Identity()});
+ observations_.PushFromBottom({
+ t,
+ t,
+ X_hat_,
+ P_,
+ Input::Zero(),
+ Output::Zero(),
+ {},
+ [](const State &, const Input &) { return Output::Zero(); },
+ [](const State &) {
+ return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero();
+ },
+ Eigen::Matrix<Scalar, kNOutputs, kNOutputs>::Identity(),
+ StateSquare::Identity(),
+ StateSquare::Zero(),
+ std::chrono::seconds(0),
+ State::Zero(),
+ });
}
// Correct with:
@@ -292,6 +309,29 @@
dt_config_.robot_radius;
}
+ // Returns the last state before the specified time.
+ // Returns nullopt if time is older than the oldest measurement.
+ std::optional<State> LastStateBeforeTime(
+ aos::monotonic_clock::time_point time) {
+ if (observations_.empty() || observations_.begin()->t > time) {
+ return std::nullopt;
+ }
+ for (const auto &observation : observations_) {
+ if (observation.t > time) {
+ // Note that observation.X_hat actually references the _previous_ X_hat.
+ return observation.X_hat;
+ }
+ }
+ return X_hat();
+ }
+
+ // Returns the most recent input vector.
+ Input MostRecentInput() {
+ CHECK(!observations_.empty());
+ Input U = observations_.top().U;
+ return U;
+ }
+
private:
struct Observation {
// Time when the observation was taken.
@@ -331,6 +371,16 @@
// The measurement noise matrix.
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
+ // Discretized A and Q to use on this update step. These will only be
+ // recalculated if the timestep changes.
+ StateSquare A_d;
+ StateSquare Q_d;
+ aos::monotonic_clock::duration discretization_time;
+
+ // A cached value indicating how much we change X_hat in the prediction step
+ // of this Observation.
+ State predict_update;
+
// In order to sort the observations in the PriorityQueue object, we
// need a comparison function.
friend bool operator<(const Observation &l, const Observation &r) {
@@ -453,45 +503,56 @@
return Xdot;
}
- void PredictImpl(const Input &U, std::chrono::nanoseconds dt, State *state,
+ void PredictImpl(Observation *obs, std::chrono::nanoseconds dt, State *state,
StateSquare *P) {
- StateSquare A_c = AForState(*state);
- StateSquare A_d;
- StateSquare Q_d;
- // TODO(james): By far the biggest CPU sink in the localization appears to
- // be this discretization--it's possible the spline code spikes higher,
- // but it doesn't create anywhere near the same sustained load. There
- // are a few potential options for optimizing this code, but none of
- // them are entirely trivial, e.g. we could:
- // -Reduce the number of states (this function grows at O(kNStates^3))
- // -Adjust the discretization function itself (there're a few things we
- // can tune there).
- // -Try to come up with some sort of lookup table or other way of
- // pre-calculating A_d and Q_d.
- // I also have to figure out how much we care about the precision of
- // some of these values--I don't think we care much, but we probably
- // do want to maintain some of the structure of the matrices.
- controls::DiscretizeQAFast(Q_continuous_, A_c, dt, &Q_d, &A_d);
+ // Only recalculate the discretization if the timestep has changed.
+ // Technically, this isn't quite correct, since the discretization will
+ // change depending on the current state. However, the slight loss of
+ // precision seems acceptable for the sake of significantly reducing CPU
+ // usage.
+ if (obs->discretization_time != dt) {
+ // TODO(james): By far the biggest CPU sink in the localization appears to
+ // be this discretization--it's possible the spline code spikes higher,
+ // but it doesn't create anywhere near the same sustained load. There
+ // are a few potential options for optimizing this code, but none of
+ // them are entirely trivial, e.g. we could:
+ // -Reduce the number of states (this function grows at O(kNStates^3))
+ // -Adjust the discretization function itself (there're a few things we
+ // can tune there).
+ // -Try to come up with some sort of lookup table or other way of
+ // pre-calculating A_d and Q_d.
+ // I also have to figure out how much we care about the precision of
+ // some of these values--I don't think we care much, but we probably
+ // do want to maintain some of the structure of the matrices.
+ const StateSquare A_c = AForState(*state);
+ controls::DiscretizeQAFast(Q_continuous_, A_c, dt, &obs->Q_d, &obs->A_d);
+ obs->discretization_time = dt;
- *state = RungeKuttaU(
- [this](const State &X, const Input &U) { return DiffEq(X, U); }, *state,
- U, aos::time::DurationInSeconds(dt));
+ obs->predict_update =
+ RungeKuttaU(
+ [this](const State &X, const Input &U) { return DiffEq(X, U); },
+ *state, obs->U, aos::time::DurationInSeconds(dt)) -
+ *state;
+ }
- StateSquare Ptemp = A_d * *P * A_d.transpose() + Q_d;
+ *state += obs->predict_update;
+
+ StateSquare Ptemp = obs->A_d * *P * obs->A_d.transpose() + obs->Q_d;
*P = Ptemp;
}
- void CorrectImpl(const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
- const Output &Z, const Output &expected_Z,
- const Eigen::Matrix<Scalar, kNOutputs, kNStates> &H,
- State *state, StateSquare *P) {
- Output err = Z - expected_Z;
- Eigen::Matrix<Scalar, kNStates, kNOutputs> PH = *P * H.transpose();
- Eigen::Matrix<Scalar, kNOutputs, kNOutputs> S = H * PH + R;
- Eigen::Matrix<Scalar, kNStates, kNOutputs> K = PH * S.inverse();
- *state += K * err;
- StateSquare Ptemp = (StateSquare::Identity() - K * H) * *P;
+ void CorrectImpl(Observation *obs, State *state, StateSquare *P) {
+ const Eigen::Matrix<Scalar, kNOutputs, kNStates> H = obs->dhdx(*state);
+ // Note: Technically, this does calculate P * H.transpose() twice. However,
+ // when I was mucking around with some things, I found that in practice
+ // putting everything into one expression and letting Eigen optimize it
+ // directly actually improved performance relative to precalculating P *
+ // H.transpose().
+ const Eigen::Matrix<Scalar, kNStates, kNOutputs> K =
+ *P * H.transpose() * (H * *P * H.transpose() + obs->R).inverse();
+ const StateSquare Ptemp = (StateSquare::Identity() - K * H) * *P;
*P = Ptemp;
+ *state += K * (obs->z - obs->h(*state, obs->U));
}
void ProcessObservation(Observation *obs, const std::chrono::nanoseconds dt,
@@ -499,14 +560,13 @@
*state = obs->X_hat;
*P = obs->P;
if (dt.count() != 0) {
- PredictImpl(obs->U, dt, state, P);
+ PredictImpl(obs, dt, state, P);
}
if (!(obs->h && obs->dhdx)) {
CHECK(obs->make_h);
obs->make_h(*state, *P, &obs->h, &obs->dhdx);
}
- CorrectImpl(obs->R, obs->z, obs->h(*state, obs->U), obs->dhdx(*state),
- state, P);
+ CorrectImpl(obs, state, P);
}
DrivetrainConfig<double> dt_config_;
@@ -547,9 +607,10 @@
"initialized.\n";
return;
}
- auto cur_it =
- observations_.PushFromBottom({t, t, State::Zero(), StateSquare::Zero(),
- Input::Zero(), z, make_h, h, dhdx, R});
+ auto cur_it = observations_.PushFromBottom(
+ {t, t, State::Zero(), StateSquare::Zero(), Input::Zero(), z, make_h, h,
+ dhdx, R, StateSquare::Identity(), StateSquare::Zero(),
+ std::chrono::seconds(0), State::Zero()});
if (cur_it == observations_.end()) {
VLOG(1) << "Camera dropped off of end with time of "
<< aos::time::DurationInSeconds(t.time_since_epoch())
@@ -560,7 +621,6 @@
<< "s.\n";
return;
}
-
// Now we populate any state information that depends on where the
// observation was inserted into the queue. X_hat and P must be populated
// from the values present in the observation *following* this one in
@@ -594,6 +654,12 @@
next_it->prev_t = cur_it->t;
cur_it->U = (U == nullptr) ? next_it->U : *U;
}
+
+ if (kFullRewindOnEverySample) {
+ next_it = observations_.begin();
+ cur_it = next_it++;
+ }
+
// Now we need to rerun the predict step from the previous to the new
// observation as well as every following correct/predict up to the current
// time.
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index c5a2d18..46b9b8b 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -383,7 +383,7 @@
// the target exactly. Instead, just run slightly past the target:
EXPECT_LT(
::std::abs(::aos::math::DiffAngle(M_PI, drivetrain_plant_.state()(2, 0))),
- 0.5);
+ 1.0);
EXPECT_GT(HPSlotLeft().abs_pos().x(), drivetrain_plant_.state().x());
EXPECT_NEAR(HPSlotLeft().abs_pos().y(), drivetrain_plant_.state().y(), 0.2);
}
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index f84be3e..7d7c83f 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -122,7 +122,7 @@
return best_data_match;
}
-void Localizer::Update(const ::Eigen::Matrix<double, 2, 1> &U,
+void Localizer::Update(const Eigen::Matrix<double, 2, 1> &U,
aos::monotonic_clock::time_point now,
double left_encoder, double right_encoder,
double gyro_rate, const Eigen::Vector3d &accel) {
@@ -251,24 +251,36 @@
AOS_LOG(WARNING, "Dropped image match due to heading mismatch.\n");
continue;
}
+ // In order to do the EKF correction, we determine the expected state based
+ // on the state at the time the image was captured; however, we insert the
+ // correction update itself at the current time. This is technically not
+ // quite correct, but saves substantial CPU usage by making it so that we
+ // don't have to constantly rewind the entire EKF history.
+ const std::optional<State> state_at_capture =
+ ekf_.LastStateBeforeTime(capture_time);
+ if (!state_at_capture.has_value()) {
+ AOS_LOG(WARNING, "Dropped image match due to age of image.\n");
+ continue;
+ }
+ const Input U = ekf_.MostRecentInput();
// For the correction step, instead of passing in the measurement directly,
// we pass in (0, 0, 0) as the measurement and then for the expected
// measurement (Zhat) we calculate the error between the implied and actual
// poses. This doesn't affect any of the math, it just makes the code a bit
// more convenient to write given the Correct() interface we already have.
ekf_.Correct(
- Eigen::Vector3f::Zero(), nullptr, {},
- [H, H_field_target, pose_robot_target](
- const State &X, const Input &) -> Eigen::Vector3f {
- const Eigen::Vector3f Z =
- CalculateImpliedPose(X, H_field_target, pose_robot_target);
+ Eigen::Vector3f::Zero(), &U, {},
+ [H, H_field_target, pose_robot_target, state_at_capture](
+ const State &, const Input &) -> Eigen::Vector3f {
+ const Eigen::Vector3f Z = CalculateImpliedPose(
+ state_at_capture.value(), H_field_target, pose_robot_target);
// Just in case we ever do encounter any, drop measurements if they
// have non-finite numbers.
if (!Z.allFinite()) {
AOS_LOG(WARNING, "Got measurement with infinites or NaNs.\n");
return Eigen::Vector3f::Zero();
}
- Eigen::Vector3f Zhat = H * X - Z;
+ Eigen::Vector3f Zhat = H * state_at_capture.value() - Z;
// Rewrap angle difference to put it back in range. Note that this
// component of the error is currently ignored (see definition of H
// above).
@@ -283,7 +295,7 @@
}
return Zhat;
},
- [H](const State &) { return H; }, R, capture_time);
+ [H](const State &) { return H; }, R, now);
}
}