Merge "Add go flatbuffer rules"
diff --git a/.bazelrc b/.bazelrc
index 1c6dbde..be65a4b 100644
--- a/.bazelrc
+++ b/.bazelrc
@@ -15,7 +15,6 @@
build --strip=never
build --noincompatible_disable_nocopts
-common --noincompatible_restrict_string_escapes
# Use the malloc we want.
build --custom_malloc=//tools/cpp:malloc
@@ -78,7 +77,7 @@
# Dump the output of the failing test to stdout.
test --test_output=errors
-build --experimental_sandbox_base=/dev/shm/
+build --sandbox_base=/dev/shm/
build --experimental_multi_threaded_digest
build --sandbox_fake_hostname=true
@@ -92,7 +91,7 @@
startup --host_jvm_args=-Dbazel.DigestFunction=SHA256
build --spawn_strategy=linux-sandbox
-build --experimental_sandbox_default_allow_network=false
+build --sandbox_default_allow_network=false
build --strategy=TypeScriptCompile=worker --strategy=AngularTemplateCompile=worker
@@ -100,7 +99,7 @@
# Note that this doesn't quite work fully, but it should. See
# https://github.com/bazelbuild/bazel/issues/6341 for ongoing discussion with
# upstream about this.
-build --javabase=@openjdk_linux_archive//:jdk --host_javabase=@openjdk_linux_archive//:jdk
+build --java_runtime_version=openjdk_9 --tool_java_runtime_version=openjdk_9
# Prevent falling back to the host JDK.
startup --noautodetect_server_javabase
diff --git a/WORKSPACE b/WORKSPACE
index 1ea9cce..6ed6b7e 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -2,6 +2,7 @@
load("@bazel_tools//tools/build_defs/repo:git.bzl", "new_git_repository")
load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive", "http_file")
+load("@bazel_tools//tools/jdk:remote_java_repository.bzl", "remote_java_repository")
load(
"//debian:python.bzl",
python_debs = "files",
@@ -614,20 +615,19 @@
)
# Java9 JDK.
-http_archive(
+remote_java_repository(
name = "openjdk_linux_archive",
- build_file_content = """
-java_runtime(
- name = 'jdk',
- srcs = glob(['**']),
- visibility = ['//visibility:public'],
-)
-""",
+ exec_compatible_with = [
+ "@platforms//cpu:x86_64",
+ "@platforms//os:linux",
+ ],
+ prefix = "openjdk",
sha256 = "f27cb933de4f9e7fe9a703486cf44c84bc8e9f138be0c270c9e5716a32367e87",
strip_prefix = "zulu9.0.7.1-jdk9.0.7-linux_x64-allmodules",
urls = [
"https://www.frc971.org/Build-Dependencies/zulu9.0.7.1-jdk9.0.7-linux_x64-allmodules.tar.gz",
],
+ version = "9",
)
local_repository(
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 11368ae..e810e3f 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -407,7 +407,9 @@
// Returns the name of the underlying queue.
const Channel *channel() const { return sender_->channel(); }
+ // TODO(austin): Deprecate the operator bool.
operator bool() const { return sender_ ? true : false; }
+ bool valid() const { return static_cast<bool>(sender_); }
// Returns the time_points that the last message was sent at.
aos::monotonic_clock::time_point monotonic_sent_time() const {
diff --git a/aos/events/event_scheduler.cc b/aos/events/event_scheduler.cc
index c06638c..97e0946 100644
--- a/aos/events/event_scheduler.cc
+++ b/aos/events/event_scheduler.cc
@@ -66,20 +66,32 @@
}
void EventScheduler::RunOnRun() {
- for (std::function<void()> &on_run : on_run_) {
- on_run();
+ while (!on_run_.empty()) {
+ std::function<void()> fn = std::move(*on_run_.begin());
+ on_run_.erase(on_run_.begin());
+ fn();
}
- on_run_.clear();
}
-void EventScheduler::RunOnStartup() {
- for (size_t i = 0; i < on_startup_.size(); ++i) {
- on_startup_[i]();
+void EventScheduler::RunOnStartup() noexcept {
+ while (!on_startup_.empty()) {
+ std::function<void()> fn = std::move(*on_startup_.begin());
+ on_startup_.erase(on_startup_.begin());
+ fn();
}
- on_startup_.clear();
}
-void EventScheduler::RunStarted() { started_(); }
+void EventScheduler::RunStarted() {
+ if (started_) {
+ started_();
+ }
+}
+
+void EventScheduler::RunStopped() {
+ if (stopped_) {
+ stopped_();
+ }
+}
std::ostream &operator<<(std::ostream &stream,
const aos::distributed_clock::time_point &now) {
@@ -119,6 +131,7 @@
rebooted.emplace_back(node_index);
CHECK_EQ(schedulers_[node_index]->boot_count() + 1,
times[node_index].boot);
+ schedulers_[node_index]->RunStopped();
schedulers_[node_index]->Shutdown();
}
}
@@ -144,6 +157,7 @@
void EventSchedulerScheduler::RunFor(distributed_clock::duration duration) {
distributed_clock::time_point end_time = now_ + duration;
logging::ScopedLogRestorer prev_logger;
+ RunOnStartup();
RunOnRun();
// Run all the sub-event-schedulers.
@@ -191,10 +205,13 @@
}
now_ = end_time;
+
+ RunStopped();
}
void EventSchedulerScheduler::Run() {
logging::ScopedLogRestorer prev_logger;
+ RunOnStartup();
RunOnRun();
// Run all the sub-event-schedulers.
while (is_running_) {
@@ -232,6 +249,8 @@
}
is_running_ = false;
+
+ RunStopped();
}
std::tuple<distributed_clock::time_point, EventScheduler *>
@@ -261,4 +280,17 @@
return std::make_tuple(min_event_time, min_scheduler);
}
+void EventSchedulerScheduler::TemporarilyStopAndRun(std::function<void()> fn) {
+ const bool was_running = is_running_;
+ if (is_running_) {
+ is_running_ = false;
+ RunStopped();
+ }
+ fn();
+ if (was_running) {
+ RunOnStartup();
+ RunOnRun();
+ }
+}
+
} // namespace aos
diff --git a/aos/events/event_scheduler.h b/aos/events/event_scheduler.h
index 55c1cf8..b64728c 100644
--- a/aos/events/event_scheduler.h
+++ b/aos/events/event_scheduler.h
@@ -126,7 +126,12 @@
started_ = std::move(callback);
}
+ void set_stopped(std::function<void()> callback) {
+ stopped_ = std::move(callback);
+ }
+
std::function<void()> started_;
+ std::function<void()> stopped_;
std::function<void()> on_shutdown_;
Token InvalidToken() { return events_list_.end(); }
@@ -138,10 +143,12 @@
void RunOnRun();
// Runs the OnStartup callbacks.
- void RunOnStartup();
+ void RunOnStartup() noexcept;
// Runs the Started callback.
void RunStarted();
+ // Runs the Started callback.
+ void RunStopped();
// Returns true if events are being handled.
inline bool is_running() const;
@@ -283,6 +290,13 @@
}
}
+ void RunStopped() {
+ CHECK(!is_running_);
+ for (EventScheduler *scheduler : schedulers_) {
+ scheduler->RunStopped();
+ }
+ }
+
void SetTimeConverter(TimeConverter *time_converter) {
time_converter->set_reboot_found(
[this](distributed_clock::time_point reboot_time,
@@ -294,6 +308,11 @@
});
}
+ // Runs the provided callback now. Stops everything, runs the callback, then
+ // starts it all up again. This lets us do operations like starting and
+ // stopping applications while running.
+ void TemporarilyStopAndRun(std::function<void()> fn);
+
private:
// Handles running the OnRun functions.
void RunOnRun() {
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index d190120..ec13f92 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -134,10 +134,10 @@
visibility = ["//visibility:public"],
deps = [
":buffer_encoder",
- ":crc32",
":logger_fbs",
"//aos:configuration_fbs",
"//aos/containers:resizeable_buffer",
+ "//aos/util:crc32",
"@com_github_google_flatbuffers//:flatbuffers",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/types:span",
@@ -465,12 +465,3 @@
gen_reflections = 1,
target_compatible_with = ["@platforms//os:linux"],
)
-
-cc_library(
- name = "crc32",
- srcs = ["crc32.cc"],
- hdrs = ["crc32.h"],
- deps = [
- "@com_google_absl//absl/types:span",
- ],
-)
diff --git a/aos/events/logging/log_reader.cc b/aos/events/logging/log_reader.cc
index e58ccd2..4e89a0e 100644
--- a/aos/events/logging/log_reader.cc
+++ b/aos/events/logging/log_reader.cc
@@ -39,6 +39,13 @@
time_estimation_buffer_seconds, 2.0,
"The time to buffer ahead in the log file to accurately reconstruct time.");
+DEFINE_string(
+ start_time, "",
+ "If set, start at this point in time in the log on the realtime clock.");
+DEFINE_string(
+ end_time, "",
+ "If set, end at this point in time in the log on the realtime clock.");
+
namespace aos {
namespace configuration {
// We don't really want to expose this publicly, but log reader doesn't really
@@ -131,6 +138,79 @@
using message_bridge::RemoteMessage;
} // namespace
+// Class to manage triggering events on the RT clock while replaying logs. Since
+// the RT clock can only change when we get a message, we only need to update
+// our timers when new messages are read.
+class EventNotifier {
+ public:
+ EventNotifier(EventLoop *event_loop, std::function<void()> fn,
+ std::string_view name,
+ realtime_clock::time_point realtime_event_time)
+ : event_loop_(event_loop),
+ fn_(std::move(fn)),
+ realtime_event_time_(realtime_event_time) {
+ CHECK(event_loop_);
+ event_timer_ = event_loop->AddTimer([this]() { HandleTime(); });
+
+ if (event_loop_->node() != nullptr) {
+ event_timer_->set_name(
+ absl::StrCat(event_loop_->node()->name()->string_view(), "_", name));
+ } else {
+ event_timer_->set_name(name);
+ }
+ }
+
+ ~EventNotifier() { event_timer_->Disable(); }
+
+ // Returns the event trigger time.
+ realtime_clock::time_point realtime_event_time() const {
+ return realtime_event_time_;
+ }
+
+ // Observes the next message and potentially calls the callback or updates the
+ // timer.
+ void ObserveNextMessage(monotonic_clock::time_point monotonic_message_time,
+ realtime_clock::time_point realtime_message_time) {
+ if (realtime_message_time < realtime_event_time_) {
+ return;
+ }
+ if (called_) {
+ return;
+ }
+
+ // Move the callback wakeup time to the correct time (or make it now if
+ // there's a gap in time) now that we know it is before the next
+ // message.
+ const monotonic_clock::time_point candidate_monotonic =
+ (realtime_event_time_ - realtime_message_time) + monotonic_message_time;
+ const monotonic_clock::time_point monotonic_now =
+ event_loop_->monotonic_now();
+ if (candidate_monotonic < monotonic_now) {
+ // Whops, time went backwards. Just do it now.
+ HandleTime();
+ } else {
+ event_timer_->Setup(candidate_monotonic);
+ }
+ }
+
+ private:
+ void HandleTime() {
+ if (!called_) {
+ called_ = true;
+ fn_();
+ }
+ }
+
+ EventLoop *event_loop_ = nullptr;
+ TimerHandler *event_timer_ = nullptr;
+ std::function<void()> fn_;
+
+ const realtime_clock::time_point realtime_event_time_ =
+ realtime_clock::min_time;
+
+ bool called_ = false;
+};
+
LogReader::LogReader(std::string_view filename,
const Configuration *replay_configuration)
: LogReader(SortParts({std::string(filename)}), replay_configuration) {}
@@ -139,6 +219,9 @@
const Configuration *replay_configuration)
: log_files_(std::move(log_files)),
replay_configuration_(replay_configuration) {
+ SetStartTime(FLAGS_start_time);
+ SetEndTime(FLAGS_end_time);
+
CHECK_GT(log_files_.size(), 0u);
{
// Validate that we have the same config everwhere. This will be true if
@@ -329,8 +412,15 @@
VLOG(1) << "Starting " << MaybeNodeName(node()) << "at time "
<< monotonic_start_time(boot_count());
- for (size_t i = 0; i < on_starts_.size(); ++i) {
- on_starts_[i]();
+ auto fn = [this]() {
+ for (size_t i = 0; i < on_starts_.size(); ++i) {
+ on_starts_[i]();
+ }
+ };
+ if (event_loop_factory_) {
+ event_loop_factory_->AllowApplicationCreationDuring(std::move(fn));
+ } else {
+ fn();
}
stopped_ = false;
started_ = true;
@@ -358,12 +448,19 @@
void LogReader::State::RunOnEnd() {
VLOG(1) << "Ending " << MaybeNodeName(node()) << "at time "
<< monotonic_start_time(boot_count());
- for (size_t i = 0; i < on_ends_.size(); ++i) {
- on_ends_[i]();
+ auto fn = [this]() {
+ for (size_t i = 0; i < on_ends_.size(); ++i) {
+ on_ends_[i]();
+ }
+ };
+ if (event_loop_factory_) {
+ event_loop_factory_->AllowApplicationCreationDuring(std::move(fn));
+ } else {
+ fn();
}
stopped_ = true;
- started_ = false;
+ started_ = true;
}
void LogReader::Register() {
@@ -397,7 +494,8 @@
node);
State *state = states_[node_index].get();
state->SetNodeEventLoopFactory(
- event_loop_factory_->GetNodeEventLoopFactory(node));
+ event_loop_factory_->GetNodeEventLoopFactory(node),
+ event_loop_factory_);
state->SetChannelCount(logged_configuration()->channels()->size());
timestamp_mappers.emplace_back(state->timestamp_mapper());
@@ -489,6 +587,11 @@
void LogReader::Register(SimulatedEventLoopFactory *event_loop_factory) {
RegisterWithoutStarting(event_loop_factory);
+ StartAfterRegister(event_loop_factory);
+}
+
+void LogReader::StartAfterRegister(
+ SimulatedEventLoopFactory *event_loop_factory) {
// We want to start the log file at the last start time of the log files
// from all the nodes. Compute how long each node's simulation needs to run
// to move time to this point.
@@ -588,6 +691,10 @@
State *state =
states_[configuration::GetNodeIndex(configuration(), node)].get();
+ if (!event_loop) {
+ state->ClearTimeFlags();
+ }
+
state->set_event_loop(event_loop);
// We don't run timing reports when trying to print out logged data, because
@@ -889,7 +996,7 @@
<< "is on the next boot, " << next_time << " now is "
<< state->monotonic_now();
CHECK(event_loop_factory_);
- state->RunOnEnd();
+ state->NotifyLogfileEnd();
return;
}
VLOG(1) << "Scheduling " << MaybeNodeName(state->event_loop()->node())
@@ -900,7 +1007,7 @@
} else {
VLOG(1) << MaybeNodeName(state->event_loop()->node())
<< "No next message, scheduling shutdown";
- state->RunOnEnd();
+ state->NotifyLogfileEnd();
// Set a timer up immediately after now to die. If we don't do this,
// then the senders waiting on the message we just read will never get
// called.
@@ -917,7 +1024,13 @@
if (state->OldestMessageTime() != BootTimestamp::max_time()) {
state->set_startup_timer(
- event_loop->AddTimer([state]() { state->RunOnStart(); }));
+ event_loop->AddTimer([state]() { state->NotifyLogfileStart(); }));
+ if (start_time_ != realtime_clock::min_time) {
+ state->SetStartTimeFlag(start_time_);
+ }
+ if (end_time_ != realtime_clock::max_time) {
+ state->SetEndTimeFlag(end_time_);
+ }
event_loop->OnRun([state]() {
BootTimestamp next_time = state->OldestMessageTime();
CHECK_EQ(next_time.boot, state->boot_count());
@@ -927,6 +1040,40 @@
}
}
+void LogReader::SetEndTime(std::string end_time) {
+ if (end_time.empty()) {
+ SetEndTime(realtime_clock::max_time);
+ } else {
+ std::optional<aos::realtime_clock::time_point> parsed_end_time =
+ aos::realtime_clock::FromString(end_time);
+ CHECK(parsed_end_time) << ": Failed to parse end time '" << end_time
+ << "'. Expected a date in the format of "
+ "2021-01-15_15-30-35.000000000.";
+ SetEndTime(*parsed_end_time);
+ }
+}
+
+void LogReader::SetEndTime(realtime_clock::time_point end_time) {
+ end_time_ = end_time;
+}
+
+void LogReader::SetStartTime(std::string start_time) {
+ if (start_time.empty()) {
+ SetStartTime(realtime_clock::min_time);
+ } else {
+ std::optional<aos::realtime_clock::time_point> parsed_start_time =
+ aos::realtime_clock::FromString(start_time);
+ CHECK(parsed_start_time) << ": Failed to parse start time '" << start_time
+ << "'. Expected a date in the format of "
+ "2021-01-15_15-30-35.000000000.";
+ SetStartTime(*parsed_start_time);
+ }
+}
+
+void LogReader::SetStartTime(realtime_clock::time_point start_time) {
+ start_time_ = start_time;
+}
+
void LogReader::Deregister() {
// Make sure that things get destroyed in the correct order, rather than
// relying on getting the order correct in the class definition.
@@ -1312,8 +1459,10 @@
}
void LogReader::State::SetNodeEventLoopFactory(
- NodeEventLoopFactory *node_event_loop_factory) {
+ NodeEventLoopFactory *node_event_loop_factory,
+ SimulatedEventLoopFactory *event_loop_factory) {
node_event_loop_factory_ = node_event_loop_factory;
+ event_loop_factory_ = event_loop_factory;
}
void LogReader::State::SetChannelCount(size_t count) {
@@ -1670,7 +1819,7 @@
return result;
}
-BootTimestamp LogReader::State::OldestMessageTime() const {
+BootTimestamp LogReader::State::OldestMessageTime() {
if (timestamp_mapper_ == nullptr) {
return BootTimestamp::max_time();
}
@@ -1680,6 +1829,12 @@
}
VLOG(2) << MaybeNodeName(node()) << "oldest message at "
<< result_ptr->monotonic_event_time.time;
+
+ if (result_ptr->monotonic_event_time.boot == boot_count()) {
+ ObserveNextMessage(result_ptr->monotonic_event_time.time,
+ result_ptr->realtime_event_time);
+ }
+
return result_ptr->monotonic_event_time;
}
@@ -1692,11 +1847,12 @@
void LogReader::State::Deregister() {
if (started_ && !stopped_) {
- RunOnEnd();
+ NotifyLogfileEnd();
}
for (size_t i = 0; i < channels_.size(); ++i) {
channels_[i].reset();
}
+ ClearTimeFlags();
channel_timestamp_loggers_.clear();
timestamp_loggers_.clear();
event_loop_unique_ptr_.reset();
@@ -1705,5 +1861,75 @@
node_event_loop_factory_ = nullptr;
}
+void LogReader::State::SetStartTimeFlag(realtime_clock::time_point start_time) {
+ if (start_time != realtime_clock::min_time) {
+ start_event_notifier_ = std::make_unique<EventNotifier>(
+ event_loop_, [this]() { NotifyFlagStart(); }, "flag_start", start_time);
+ }
+}
+
+void LogReader::State::SetEndTimeFlag(realtime_clock::time_point end_time) {
+ if (end_time != realtime_clock::max_time) {
+ end_event_notifier_ = std::make_unique<EventNotifier>(
+ event_loop_, [this]() { NotifyFlagEnd(); }, "flag_end", end_time);
+ }
+}
+
+void LogReader::State::ObserveNextMessage(
+ monotonic_clock::time_point monotonic_event,
+ realtime_clock::time_point realtime_event) {
+ if (start_event_notifier_) {
+ start_event_notifier_->ObserveNextMessage(monotonic_event, realtime_event);
+ }
+ if (end_event_notifier_) {
+ end_event_notifier_->ObserveNextMessage(monotonic_event, realtime_event);
+ }
+}
+
+void LogReader::State::ClearTimeFlags() {
+ start_event_notifier_.reset();
+ end_event_notifier_.reset();
+}
+
+void LogReader::State::NotifyLogfileStart() {
+ if (start_event_notifier_) {
+ if (start_event_notifier_->realtime_event_time() >
+ realtime_start_time(boot_count())) {
+ VLOG(1) << "Skipping, " << start_event_notifier_->realtime_event_time()
+ << " > " << realtime_start_time(boot_count());
+ return;
+ }
+ }
+ if (found_last_message_) {
+ VLOG(1) << "Last message already found, bailing";
+ return;
+ }
+ RunOnStart();
+}
+
+void LogReader::State::NotifyFlagStart() {
+ if (start_event_notifier_->realtime_event_time() >=
+ realtime_start_time(boot_count())) {
+ RunOnStart();
+ }
+}
+
+void LogReader::State::NotifyLogfileEnd() {
+ if (found_last_message_) {
+ return;
+ }
+
+ if (!stopped_ && started_) {
+ RunOnEnd();
+ }
+}
+
+void LogReader::State::NotifyFlagEnd() {
+ if (!stopped_ && started_) {
+ RunOnEnd();
+ SetFoundLastMessage(true);
+ }
+}
+
} // namespace logger
} // namespace aos
diff --git a/aos/events/logging/log_reader.h b/aos/events/logging/log_reader.h
index 3eebba7..9bfc90a 100644
--- a/aos/events/logging/log_reader.h
+++ b/aos/events/logging/log_reader.h
@@ -23,6 +23,8 @@
namespace aos {
namespace logger {
+class EventNotifier;
+
// We end up with one of the following 3 log file types.
//
// Single node logged as the source node.
@@ -73,10 +75,20 @@
// below, but can be anything as long as the locations needed to send
// everything are available.
void Register(SimulatedEventLoopFactory *event_loop_factory);
+
// Registers all the callbacks to send the log file data out to an event loop
// factory. This does not start replaying or change the current distributed
// time of the factory. It does change the monotonic clocks to be right.
void RegisterWithoutStarting(SimulatedEventLoopFactory *event_loop_factory);
+ // Runs the log until the last start time. Register above is defined as:
+ // Register(...) {
+ // RegisterWithoutStarting
+ // StartAfterRegister
+ // }
+ // This should generally be considered as a stepping stone to convert from
+ // Register() to RegisterWithoutStarting() incrementally.
+ void StartAfterRegister(SimulatedEventLoopFactory *event_loop_factory);
+
// Creates an SimulatedEventLoopFactory accessible via event_loop_factory(),
// and then calls Register.
void Register();
@@ -121,6 +133,14 @@
realtime_clock::time_point realtime_start_time(
const Node *node = nullptr) const;
+ // Sets the start and end times to replay data until for all nodes. This
+ // overrides the --start_time and --end_time flags. The default is to replay
+ // all data.
+ void SetStartTime(std::string start_time);
+ void SetStartTime(realtime_clock::time_point start_time);
+ void SetEndTime(std::string end_time);
+ void SetEndTime(realtime_clock::time_point end_time);
+
// Causes the logger to publish the provided channel on a different name so
// that replayed applications can publish on the proper channel name without
// interference. This operates on raw channel names, without any node or
@@ -277,7 +297,7 @@
TimestampedMessage PopOldest();
// Returns the monotonic time of the oldest message.
- BootTimestamp OldestMessageTime() const;
+ BootTimestamp OldestMessageTime();
size_t boot_count() const {
// If we are replaying directly into an event loop, we can't reboot. So
@@ -296,7 +316,7 @@
if (start_time == monotonic_clock::min_time) {
LOG(ERROR)
<< "No start time, skipping, please figure out when this happens";
- RunOnStart();
+ NotifyLogfileStart();
return;
}
CHECK_GE(start_time, event_loop_->monotonic_now());
@@ -329,7 +349,9 @@
// Sets the node event loop factory for replaying into a
// SimulatedEventLoopFactory. Returns the EventLoop to use.
- void SetNodeEventLoopFactory(NodeEventLoopFactory *node_event_loop_factory);
+ void SetNodeEventLoopFactory(
+ NodeEventLoopFactory *node_event_loop_factory,
+ SimulatedEventLoopFactory *event_loop_factory);
// Sets and gets the event loop to use.
void set_event_loop(EventLoop *event_loop) { event_loop_ = event_loop; }
@@ -419,6 +441,18 @@
void RunOnStart();
void RunOnEnd();
+ // Handles a logfile start event to potentially call the OnStart callbacks.
+ void NotifyLogfileStart();
+ // Handles a start time flag start event to potentially call the OnStart
+ // callbacks.
+ void NotifyFlagStart();
+
+ // Handles a logfile end event to potentially call the OnEnd callbacks.
+ void NotifyLogfileEnd();
+ // Handles a end time flag start event to potentially call the OnEnd
+ // callbacks.
+ void NotifyFlagEnd();
+
// Unregisters everything so we can destory the event loop.
// TODO(austin): Is this needed? OnShutdown should be able to serve this
// need.
@@ -437,6 +471,18 @@
}
}
+ // Creates and registers the --start_time and --end_time event callbacks.
+ void SetStartTimeFlag(realtime_clock::time_point start_time);
+ void SetEndTimeFlag(realtime_clock::time_point end_time);
+
+ // Notices the next message to update the start/end time callbacks.
+ void ObserveNextMessage(monotonic_clock::time_point monotonic_event,
+ realtime_clock::time_point realtime_event);
+
+ // Clears the start and end time flag handlers so we can delete the event
+ // loop.
+ void ClearTimeFlags();
+
// Sets the next wakeup time on the replay callback.
void Setup(monotonic_clock::time_point next_time) {
timer_handler_->Setup(next_time);
@@ -516,6 +562,8 @@
// Factory (if we are in sim) that this loop was created on.
NodeEventLoopFactory *node_event_loop_factory_ = nullptr;
+ SimulatedEventLoopFactory *event_loop_factory_ = nullptr;
+
std::unique_ptr<EventLoop> event_loop_unique_ptr_;
// Event loop.
const Node *node_ = nullptr;
@@ -524,6 +572,9 @@
TimerHandler *timer_handler_ = nullptr;
TimerHandler *startup_timer_ = nullptr;
+ std::unique_ptr<EventNotifier> start_event_notifier_;
+ std::unique_ptr<EventNotifier> end_event_notifier_;
+
// Filters (or nullptr if it isn't a forwarded channel) for each channel.
// This corresponds to the object which is shared among all the channels
// going between 2 nodes. The second element in the tuple indicates if this
@@ -598,6 +649,9 @@
// Whether to exit the SimulatedEventLoop when we finish reading the logs.
bool exit_on_finish_ = true;
+
+ realtime_clock::time_point start_time_ = realtime_clock::min_time;
+ realtime_clock::time_point end_time_ = realtime_clock::max_time;
};
} // namespace logger
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 1732549..3b0b672 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -523,7 +523,12 @@
}
};
-void ConfirmReadable(const std::vector<std::string> &files) {
+std::vector<std::pair<std::vector<realtime_clock::time_point>,
+ std::vector<realtime_clock::time_point>>>
+ConfirmReadable(
+ const std::vector<std::string> &files,
+ realtime_clock::time_point start_time = realtime_clock::min_time,
+ realtime_clock::time_point end_time = realtime_clock::max_time) {
{
LogReader reader(SortParts(files));
@@ -535,22 +540,65 @@
reader.Deregister();
}
{
+ std::vector<std::pair<std::vector<realtime_clock::time_point>,
+ std::vector<realtime_clock::time_point>>>
+ result;
LogReader reader(SortParts(files));
+ reader.SetStartTime(start_time);
+ reader.SetEndTime(end_time);
+
SimulatedEventLoopFactory log_reader_factory(reader.configuration());
reader.RegisterWithoutStarting(&log_reader_factory);
+ result.resize(
+ configuration::NodesCount(log_reader_factory.configuration()));
if (configuration::MultiNode(log_reader_factory.configuration())) {
+ size_t i = 0;
for (const aos::Node *node :
*log_reader_factory.configuration()->nodes()) {
- reader.OnStart(node, [node]() {
+ LOG(INFO) << "Registering start";
+ reader.OnStart(node, [node, &log_reader_factory, &result,
+ node_index = i]() {
LOG(INFO) << "Starting " << node->name()->string_view();
+ result[node_index].first.push_back(
+ log_reader_factory.GetNodeEventLoopFactory(node)->realtime_now());
});
+ reader.OnEnd(node, [node, &log_reader_factory, &result,
+ node_index = i]() {
+ LOG(INFO) << "Ending " << node->name()->string_view();
+ result[node_index].second.push_back(
+ log_reader_factory.GetNodeEventLoopFactory(node)->realtime_now());
+ });
+ ++i;
}
+ } else {
+ reader.OnStart([&log_reader_factory, &result]() {
+ LOG(INFO) << "Starting";
+ result[0].first.push_back(
+ log_reader_factory.GetNodeEventLoopFactory(nullptr)
+ ->realtime_now());
+ });
+ reader.OnEnd([&log_reader_factory, &result]() {
+ LOG(INFO) << "Ending";
+ result[0].second.push_back(
+ log_reader_factory.GetNodeEventLoopFactory(nullptr)
+ ->realtime_now());
+ });
}
log_reader_factory.Run();
reader.Deregister();
+
+ for (auto x : result) {
+ for (auto y : x.first) {
+ VLOG(1) << "Start " << y;
+ }
+ for (auto y : x.second) {
+ VLOG(1) << "End " << y;
+ }
+ }
+ return result;
}
}
@@ -3589,7 +3637,27 @@
// Confirm that we can parse the result. LogReader has enough internal CHECKs
// to confirm the right thing happened.
const std::vector<LogFile> sorted_parts = SortParts(filenames);
- ConfirmReadable(filenames);
+ auto result = ConfirmReadable(filenames);
+ EXPECT_THAT(result[0].first, ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::seconds(1)));
+ EXPECT_THAT(result[0].second,
+ ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::microseconds(34990350)));
+
+ EXPECT_THAT(result[1].first,
+ ::testing::ElementsAre(
+ realtime_clock::epoch() + chrono::seconds(1),
+ realtime_clock::epoch() + chrono::microseconds(3323000)));
+ EXPECT_THAT(result[1].second,
+ ::testing::ElementsAre(
+ realtime_clock::epoch() + chrono::microseconds(13990200),
+ realtime_clock::epoch() + chrono::microseconds(16313200)));
+
+ EXPECT_THAT(result[2].first, ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::seconds(1)));
+ EXPECT_THAT(result[2].second,
+ ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::microseconds(34900150)));
}
// Tests that local data before remote data after reboot is properly replayed.
@@ -3730,9 +3798,201 @@
// Confirm that we can parse the result. LogReader has enough internal CHECKs
// to confirm the right thing happened.
const std::vector<LogFile> sorted_parts = SortParts(filenames);
- ConfirmReadable(filenames);
+ auto result = ConfirmReadable(filenames);
+
+ EXPECT_THAT(result[0].first, ::testing::ElementsAre(realtime_clock::epoch()));
+ EXPECT_THAT(result[0].second,
+ ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::microseconds(11000350)));
+
+ EXPECT_THAT(result[1].first,
+ ::testing::ElementsAre(
+ realtime_clock::epoch(),
+ realtime_clock::epoch() + chrono::microseconds(107005000)));
+ EXPECT_THAT(result[1].second,
+ ::testing::ElementsAre(
+ realtime_clock::epoch() + chrono::microseconds(4000150),
+ realtime_clock::epoch() + chrono::microseconds(111000200)));
+
+ EXPECT_THAT(result[2].first, ::testing::ElementsAre(realtime_clock::epoch()));
+ EXPECT_THAT(result[2].second,
+ ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::microseconds(11000150)));
+
+ auto start_stop_result = ConfirmReadable(
+ filenames, realtime_clock::epoch() + chrono::milliseconds(2000),
+ realtime_clock::epoch() + chrono::milliseconds(3000));
+
+ EXPECT_THAT(start_stop_result[0].first, ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::seconds(2)));
+ EXPECT_THAT(start_stop_result[0].second, ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::seconds(3)));
+ EXPECT_THAT(start_stop_result[1].first, ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::seconds(2)));
+ EXPECT_THAT(start_stop_result[1].second, ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::seconds(3)));
+ EXPECT_THAT(start_stop_result[2].first, ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::seconds(2)));
+ EXPECT_THAT(start_stop_result[2].second, ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::seconds(3)));
}
+// Tests that setting the start and stop flags across a reboot works as
+// expected.
+TEST(MultinodeRebootLoggerTest, RebootStartStopTimes) {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(ArtifactPath(
+ "aos/events/logging/multinode_pingpong_split3_config.json"));
+ message_bridge::TestingTimeConverter time_converter(
+ configuration::NodesCount(&config.message()));
+ SimulatedEventLoopFactory event_loop_factory(&config.message());
+ event_loop_factory.SetTimeConverter(&time_converter);
+ NodeEventLoopFactory *const pi1 =
+ event_loop_factory.GetNodeEventLoopFactory("pi1");
+ const size_t pi1_index = configuration::GetNodeIndex(
+ event_loop_factory.configuration(), pi1->node());
+ NodeEventLoopFactory *const pi2 =
+ event_loop_factory.GetNodeEventLoopFactory("pi2");
+ const size_t pi2_index = configuration::GetNodeIndex(
+ event_loop_factory.configuration(), pi2->node());
+ NodeEventLoopFactory *const pi3 =
+ event_loop_factory.GetNodeEventLoopFactory("pi3");
+ const size_t pi3_index = configuration::GetNodeIndex(
+ event_loop_factory.configuration(), pi3->node());
+
+ const std::string kLogfile1_1 =
+ aos::testing::TestTmpDir() + "/multi_logfile1/";
+ const std::string kLogfile2_1 =
+ aos::testing::TestTmpDir() + "/multi_logfile2.1/";
+ const std::string kLogfile2_2 =
+ aos::testing::TestTmpDir() + "/multi_logfile2.2/";
+ const std::string kLogfile3_1 =
+ aos::testing::TestTmpDir() + "/multi_logfile3/";
+ util::UnlinkRecursive(kLogfile1_1);
+ util::UnlinkRecursive(kLogfile2_1);
+ util::UnlinkRecursive(kLogfile2_2);
+ util::UnlinkRecursive(kLogfile3_1);
+ {
+ CHECK_EQ(pi1_index, 0u);
+ CHECK_EQ(pi2_index, 1u);
+ CHECK_EQ(pi3_index, 2u);
+
+ time_converter.AddNextTimestamp(
+ distributed_clock::epoch(),
+ {BootTimestamp::epoch(), BootTimestamp::epoch(),
+ BootTimestamp::epoch()});
+ const chrono::nanoseconds reboot_time = chrono::milliseconds(5000);
+ time_converter.AddNextTimestamp(
+ distributed_clock::epoch() + reboot_time,
+ {BootTimestamp::epoch() + reboot_time,
+ BootTimestamp{.boot = 1,
+ .time = monotonic_clock::epoch() + reboot_time},
+ BootTimestamp::epoch() + reboot_time});
+ }
+
+ std::vector<std::string> filenames;
+ {
+ LoggerState pi1_logger = LoggerState::MakeLogger(
+ pi1, &event_loop_factory, SupportedCompressionAlgorithms()[0]);
+ LoggerState pi3_logger = LoggerState::MakeLogger(
+ pi3, &event_loop_factory, SupportedCompressionAlgorithms()[0]);
+ {
+ // And now start the logger.
+ LoggerState pi2_logger = LoggerState::MakeLogger(
+ pi2, &event_loop_factory, SupportedCompressionAlgorithms()[0]);
+
+ pi1_logger.StartLogger(kLogfile1_1);
+ pi3_logger.StartLogger(kLogfile3_1);
+ pi2_logger.StartLogger(kLogfile2_1);
+
+ event_loop_factory.RunFor(chrono::milliseconds(1005));
+
+ // Now that we've got a start time in the past, turn on data.
+ std::unique_ptr<aos::EventLoop> ping_event_loop =
+ pi1->MakeEventLoop("ping");
+ Ping ping(ping_event_loop.get());
+
+ pi2->AlwaysStart<Pong>("pong");
+
+ event_loop_factory.RunFor(chrono::milliseconds(3000));
+
+ pi2_logger.AppendAllFilenames(&filenames);
+ }
+ event_loop_factory.RunFor(chrono::milliseconds(995));
+ // pi2 now reboots at 5 seconds.
+ {
+ event_loop_factory.RunFor(chrono::milliseconds(1000));
+
+ // Make local stuff happen before we start logging and connect the remote.
+ pi2->AlwaysStart<Pong>("pong");
+ std::unique_ptr<aos::EventLoop> ping_event_loop =
+ pi1->MakeEventLoop("ping");
+ Ping ping(ping_event_loop.get());
+ event_loop_factory.RunFor(chrono::milliseconds(5));
+
+ // Start logging again on pi2 after it is up.
+ LoggerState pi2_logger = LoggerState::MakeLogger(
+ pi2, &event_loop_factory, SupportedCompressionAlgorithms()[0]);
+ pi2_logger.StartLogger(kLogfile2_2);
+
+ event_loop_factory.RunFor(chrono::milliseconds(5000));
+
+ pi2_logger.AppendAllFilenames(&filenames);
+ }
+
+ pi1_logger.AppendAllFilenames(&filenames);
+ pi3_logger.AppendAllFilenames(&filenames);
+ }
+
+ const std::vector<LogFile> sorted_parts = SortParts(filenames);
+ auto result = ConfirmReadable(filenames);
+
+ EXPECT_THAT(result[0].first, ::testing::ElementsAre(realtime_clock::epoch()));
+ EXPECT_THAT(result[0].second,
+ ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::microseconds(11000350)));
+
+ EXPECT_THAT(result[1].first,
+ ::testing::ElementsAre(
+ realtime_clock::epoch(),
+ realtime_clock::epoch() + chrono::microseconds(6005000)));
+ EXPECT_THAT(result[1].second,
+ ::testing::ElementsAre(
+ realtime_clock::epoch() + chrono::microseconds(4900150),
+ realtime_clock::epoch() + chrono::microseconds(11000200)));
+
+ EXPECT_THAT(result[2].first, ::testing::ElementsAre(realtime_clock::epoch()));
+ EXPECT_THAT(result[2].second,
+ ::testing::ElementsAre(realtime_clock::epoch() +
+ chrono::microseconds(11000150)));
+
+ // Confirm we observed the correct start and stop times. We should see the
+ // reboot here.
+ auto start_stop_result = ConfirmReadable(
+ filenames, realtime_clock::epoch() + chrono::milliseconds(2000),
+ realtime_clock::epoch() + chrono::milliseconds(8000));
+
+ EXPECT_THAT(
+ start_stop_result[0].first,
+ ::testing::ElementsAre(realtime_clock::epoch() + chrono::seconds(2)));
+ EXPECT_THAT(
+ start_stop_result[0].second,
+ ::testing::ElementsAre(realtime_clock::epoch() + chrono::seconds(8)));
+ EXPECT_THAT(start_stop_result[1].first,
+ ::testing::ElementsAre(
+ realtime_clock::epoch() + chrono::seconds(2),
+ realtime_clock::epoch() + chrono::microseconds(6005000)));
+ EXPECT_THAT(start_stop_result[1].second,
+ ::testing::ElementsAre(
+ realtime_clock::epoch() + chrono::microseconds(4900150),
+ realtime_clock::epoch() + chrono::seconds(8)));
+ EXPECT_THAT(
+ start_stop_result[2].first,
+ ::testing::ElementsAre(realtime_clock::epoch() + chrono::seconds(2)));
+ EXPECT_THAT(
+ start_stop_result[2].second,
+ ::testing::ElementsAre(realtime_clock::epoch() + chrono::seconds(8)));
+}
} // namespace testing
} // namespace logger
diff --git a/aos/events/logging/snappy_encoder.cc b/aos/events/logging/snappy_encoder.cc
index 4621273..8f46cf3 100644
--- a/aos/events/logging/snappy_encoder.cc
+++ b/aos/events/logging/snappy_encoder.cc
@@ -1,6 +1,6 @@
#include "aos/events/logging/snappy_encoder.h"
-#include "aos/events/logging/crc32.h"
+#include "aos/util/crc32.h"
#include "external/snappy/snappy.h"
namespace aos::logger {
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index a2428a4..a1ef95d 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -1268,6 +1268,11 @@
event_loop->SetIsRunning(true);
}
});
+ scheduler_.set_stopped([this]() {
+ for (SimulatedEventLoop *event_loop : event_loops_) {
+ event_loop->SetIsRunning(false);
+ }
+ });
scheduler_.set_on_shutdown([this]() {
VLOG(1) << scheduler_.distributed_now() << " " << NodeName(this->node())
<< monotonic_now() << " Shutting down node.";
@@ -1339,7 +1344,7 @@
void NodeEventLoopFactory::Shutdown() {
for (SimulatedEventLoop *event_loop : event_loops_) {
- event_loop->SetIsRunning(false);
+ CHECK(!event_loop->is_running());
}
CHECK(started_);
@@ -1367,12 +1372,11 @@
void SimulatedEventLoopFactory::RunFor(monotonic_clock::duration duration) {
// This sets running to true too.
- scheduler_scheduler_.RunOnStartup();
scheduler_scheduler_.RunFor(duration);
for (std::unique_ptr<NodeEventLoopFactory> &node : node_factories_) {
if (node) {
for (SimulatedEventLoop *loop : node->event_loops_) {
- loop->SetIsRunning(false);
+ CHECK(!loop->is_running());
}
}
}
@@ -1380,12 +1384,11 @@
void SimulatedEventLoopFactory::Run() {
// This sets running to true too.
- scheduler_scheduler_.RunOnStartup();
scheduler_scheduler_.Run();
for (std::unique_ptr<NodeEventLoopFactory> &node : node_factories_) {
if (node) {
for (SimulatedEventLoop *loop : node->event_loops_) {
- loop->SetIsRunning(false);
+ CHECK(!loop->is_running());
}
}
}
@@ -1458,6 +1461,11 @@
return std::move(result);
}
+void SimulatedEventLoopFactory::AllowApplicationCreationDuring(
+ std::function<void()> fn) {
+ scheduler_scheduler_.TemporarilyStopAndRun(std::move(fn));
+}
+
void NodeEventLoopFactory::Disconnect(const Node *other) {
factory_->bridge_->Disconnect(node_, other);
}
diff --git a/aos/events/simulated_event_loop.h b/aos/events/simulated_event_loop.h
index bd589d6..9a447e9 100644
--- a/aos/events/simulated_event_loop.h
+++ b/aos/events/simulated_event_loop.h
@@ -128,6 +128,11 @@
// tests.
void SkipTimingReport();
+ // Re-enables application creation for the duration of fn. This is mostly to
+ // allow use cases like log reading to create applications after the node
+ // starts up without stopping execution.
+ void AllowApplicationCreationDuring(std::function<void()> fn);
+
private:
friend class NodeEventLoopFactory;
diff --git a/aos/events/simulated_event_loop_test.cc b/aos/events/simulated_event_loop_test.cc
index a94b895..09202ff 100644
--- a/aos/events/simulated_event_loop_test.cc
+++ b/aos/events/simulated_event_loop_test.cc
@@ -181,6 +181,29 @@
EXPECT_EQ(counter, 0);
}
+// Test that TemporarilyStopAndRun respects and preserves running.
+TEST(EventSchedulerTest, TemporarilyStopAndRun) {
+ int counter = 0;
+ EventSchedulerScheduler scheduler_scheduler;
+ EventScheduler scheduler(0);
+ scheduler_scheduler.AddEventScheduler(&scheduler);
+
+ scheduler_scheduler.TemporarilyStopAndRun(
+ [&]() { CHECK(!scheduler_scheduler.is_running()); });
+ ASSERT_FALSE(scheduler_scheduler.is_running());
+
+ FunctionEvent e([&]() {
+ counter += 1;
+ CHECK(scheduler_scheduler.is_running());
+ scheduler_scheduler.TemporarilyStopAndRun(
+ [&]() { CHECK(!scheduler_scheduler.is_running()); });
+ CHECK(scheduler_scheduler.is_running());
+ });
+ scheduler.Schedule(monotonic_clock::epoch() + chrono::seconds(1), &e);
+ scheduler_scheduler.Run();
+ EXPECT_EQ(counter, 1);
+}
+
void SendTestMessage(aos::Sender<TestMessage> *sender, int value) {
aos::Sender<TestMessage>::Builder builder = sender->MakeBuilder();
TestMessage::Builder test_message_builder =
diff --git a/aos/network/log_web_proxy_main.cc b/aos/network/log_web_proxy_main.cc
index 5c099a2..945d034 100644
--- a/aos/network/log_web_proxy_main.cc
+++ b/aos/network/log_web_proxy_main.cc
@@ -42,7 +42,8 @@
event_loop->SkipTimingReport();
- aos::web_proxy::WebProxy web_proxy(event_loop.get(), FLAGS_buffer_size);
+ aos::web_proxy::WebProxy web_proxy(
+ event_loop.get(), aos::web_proxy::StoreHistory::kYes, FLAGS_buffer_size);
web_proxy.SetDataPath(FLAGS_data_dir.c_str());
diff --git a/aos/network/web_proxy.cc b/aos/network/web_proxy.cc
index fdd8f1e..6d4f23f 100644
--- a/aos/network/web_proxy.cc
+++ b/aos/network/web_proxy.cc
@@ -28,7 +28,9 @@
namespace aos {
namespace web_proxy {
WebsocketHandler::WebsocketHandler(::seasocks::Server *server,
- aos::EventLoop *event_loop, int buffer_size)
+ aos::EventLoop *event_loop,
+ StoreHistory store_history,
+ int per_channel_buffer_size_bytes)
: server_(server),
config_(aos::CopyFlatBuffer(event_loop->configuration())),
event_loop_(event_loop) {
@@ -47,7 +49,10 @@
if (aos::configuration::ChannelIsReadableOnNode(channel, self)) {
auto fetcher = event_loop_->MakeRawFetcher(channel);
subscribers_.emplace_back(std::make_unique<aos::web_proxy::Subscriber>(
- std::move(fetcher), i, buffer_size));
+ std::move(fetcher), i, store_history,
+ per_channel_buffer_size_bytes < 0
+ ? -1
+ : per_channel_buffer_size_bytes / channel->max_size()));
} else {
subscribers_.emplace_back(nullptr);
}
@@ -126,19 +131,24 @@
global_epoll->DeleteFd(fd);
}
-WebProxy::WebProxy(aos::EventLoop *event_loop, int buffer_size)
- : WebProxy(event_loop, &internal_epoll_, buffer_size) {}
+WebProxy::WebProxy(aos::EventLoop *event_loop, StoreHistory store_history,
+ int per_channel_buffer_size_bytes)
+ : WebProxy(event_loop, &internal_epoll_, store_history,
+ per_channel_buffer_size_bytes) {}
-WebProxy::WebProxy(aos::ShmEventLoop *event_loop, int buffer_size)
- : WebProxy(event_loop, event_loop->epoll(), buffer_size) {}
+WebProxy::WebProxy(aos::ShmEventLoop *event_loop, StoreHistory store_history,
+ int per_channel_buffer_size_bytes)
+ : WebProxy(event_loop, event_loop->epoll(), store_history,
+ per_channel_buffer_size_bytes) {}
WebProxy::WebProxy(aos::EventLoop *event_loop, aos::internal::EPoll *epoll,
- int buffer_size)
+ StoreHistory store_history,
+ int per_channel_buffer_size_bytes)
: epoll_(epoll),
server_(std::make_shared<aos::seasocks::SeasocksLogger>(
::seasocks::Logger::Level::Info)),
- websocket_handler_(
- new WebsocketHandler(&server_, event_loop, buffer_size)) {
+ websocket_handler_(new WebsocketHandler(
+ &server_, event_loop, store_history, per_channel_buffer_size_bytes)) {
CHECK(!global_epoll);
global_epoll = epoll;
@@ -192,10 +202,13 @@
}
void Subscriber::RunIteration() {
- if (channels_.empty() && buffer_size_ == 0) {
+ if (channels_.empty() && (buffer_size_ == 0 || !store_history_)) {
+ fetcher_->Fetch();
+ message_buffer_.clear();
return;
}
+
while (fetcher_->FetchNext()) {
// If we aren't building up a buffer, short-circuit the FetchNext().
if (buffer_size_ == 0) {
@@ -271,12 +284,6 @@
ChannelInformation info;
info.transfer_method = transfer_method;
- // If we aren't keeping a buffer and there are no existing listeners, call
- // Fetch() to avoid falling behind on future calls to FetchNext().
- if (channels_.empty() && buffer_size_ == 0) {
- fetcher_->Fetch();
- }
-
channels_.emplace_back(std::make_pair(data_channel, info));
data_channel->set_on_message(
diff --git a/aos/network/web_proxy.fbs b/aos/network/web_proxy.fbs
index 6c85acb..f1d6645 100644
--- a/aos/network/web_proxy.fbs
+++ b/aos/network/web_proxy.fbs
@@ -64,7 +64,7 @@
enum TransferMethod : byte {
SUBSAMPLE,
- EVERYTHING_WITH_HISTORY,
+ LOSSLESS,
}
table ChannelRequest {
diff --git a/aos/network/web_proxy.h b/aos/network/web_proxy.h
index 0815ebf..baca26e 100644
--- a/aos/network/web_proxy.h
+++ b/aos/network/web_proxy.h
@@ -24,13 +24,19 @@
class Subscriber;
class ApplicationConnection;
+enum class StoreHistory {
+ kNo,
+ kYes,
+};
+
// Basic class that handles receiving new websocket connections. Creates a new
// Connection to manage the rest of the negotiation and data passing. When the
// websocket closes, it deletes the Connection.
class WebsocketHandler : public ::seasocks::WebSocket::Handler {
public:
WebsocketHandler(::seasocks::Server *server, aos::EventLoop *event_loop,
- int buffer_size);
+ StoreHistory store_history,
+ int per_channel_buffer_size_bytes);
void onConnect(::seasocks::WebSocket *sock) override;
void onData(::seasocks::WebSocket *sock, const uint8_t *data,
size_t size) override;
@@ -50,15 +56,28 @@
// Wrapper class that manages the seasocks server and WebsocketHandler.
class WebProxy {
public:
- WebProxy(aos::EventLoop *event_loop, int buffer_size);
- WebProxy(aos::ShmEventLoop *event_loop, int buffer_size);
+ // Constructs a WebProxy object for interacting with a webpage. store_history
+ // and per_channel_buffer_size_bytes specify how we manage delivering LOSSLESS
+ // messages to the client:
+ // * store_history specifies whether we should always buffer up data for all
+ // channels--even for messages that are played prior to the client
+ // connecting. This is mostly useful for log replay where the client
+ // will typically connect after the logfile has been fully loaded/replayed.
+ // * per_channel_buffer_size_bytes is the maximum amount of data to buffer
+ // up per channel (-1 will indicate infinite data, which is used during log
+ // replay). This is divided by the max_size per channel to determine
+ // how many messages to queue up.
+ WebProxy(aos::EventLoop *event_loop, StoreHistory store_history,
+ int per_channel_buffer_size_bytes);
+ WebProxy(aos::ShmEventLoop *event_loop, StoreHistory store_history,
+ int per_channel_buffer_size_bytes);
~WebProxy();
void SetDataPath(const char *path) { server_.setStaticPath(path); }
private:
WebProxy(aos::EventLoop *event_loop, aos::internal::EPoll *epoll,
- int buffer_size);
+ StoreHistory store_history, int per_channel_buffer_size_bytes);
aos::internal::EPoll internal_epoll_;
aos::internal::EPoll *const epoll_;
@@ -96,9 +115,10 @@
class Subscriber {
public:
Subscriber(std::unique_ptr<RawFetcher> fetcher, int channel_index,
- int buffer_size)
+ StoreHistory store_history, int buffer_size)
: fetcher_(std::move(fetcher)),
channel_index_(channel_index),
+ store_history_(store_history == StoreHistory::kYes),
buffer_size_(buffer_size) {}
void RunIteration();
@@ -133,6 +153,10 @@
std::unique_ptr<RawFetcher> fetcher_;
int channel_index_;
+ // If set, will always build up a buffer of the most recent buffer_size_
+ // messages. If store_history_ is *not* set we will only buffer up messages
+ // while there is an active listener.
+ bool store_history_;
int buffer_size_;
std::deque<Message> message_buffer_;
// The ScopedDataChannel that we use for actually sending data over WebRTC
diff --git a/aos/network/web_proxy_main.cc b/aos/network/web_proxy_main.cc
index 06fe942..f3ad926 100644
--- a/aos/network/web_proxy_main.cc
+++ b/aos/network/web_proxy_main.cc
@@ -6,7 +6,9 @@
DEFINE_string(config, "./config.json", "File path of aos configuration");
DEFINE_string(data_dir, "www", "Directory to serve data files from");
-DEFINE_int32(buffer_size, 0, "-1 if infinite, in # of messages / channel.");
+DEFINE_int32(buffer_size, 1000000,
+ "-1 if infinite, in bytes / channel. If there are no active "
+ "connections, will not store anything.");
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
@@ -16,7 +18,8 @@
aos::ShmEventLoop event_loop(&config.message());
- aos::web_proxy::WebProxy web_proxy(&event_loop, FLAGS_buffer_size);
+ aos::web_proxy::WebProxy web_proxy(
+ &event_loop, aos::web_proxy::StoreHistory::kNo, FLAGS_buffer_size);
web_proxy.SetDataPath(FLAGS_data_dir.c_str());
event_loop.Run();
diff --git a/aos/network/www/proxy.ts b/aos/network/www/proxy.ts
index 5461899..5415400 100644
--- a/aos/network/www/proxy.ts
+++ b/aos/network/www/proxy.ts
@@ -117,7 +117,7 @@
name: string, type: string,
handler: (data: Uint8Array, sentTime: number) => void): void {
this.addHandlerImpl(
- name, type, TransferMethod.EVERYTHING_WITH_HISTORY, handler);
+ name, type, TransferMethod.LOSSLESS, handler);
}
/**
@@ -137,7 +137,7 @@
if (!this.handlerFuncs.has(channel.key())) {
this.handlerFuncs.set(channel.key(), []);
} else {
- if (method == TransferMethod.EVERYTHING_WITH_HISTORY) {
+ if (method == TransferMethod.LOSSLESS) {
console.warn(
'Behavior of multiple reliable handlers is currently poorly ' +
'defined and may not actually deliver all of the messages.');
diff --git a/aos/starter/starter.sh b/aos/starter/starter.sh
index 7a25fc7..f18dfda 100755
--- a/aos/starter/starter.sh
+++ b/aos/starter/starter.sh
@@ -8,6 +8,43 @@
ln -s /var/local/natinst/log/FRC_UserProgram.log /tmp/FRC_UserProgram.log
ln -s /var/local/natinst/log/FRC_UserProgram.log "${ROBOT_CODE}/FRC_UserProgram.log"
elif [[ "$(hostname)" == "pi-"* ]]; then
+ function chrtirq() {
+ ps -ef | grep "\\[$1\\]" | awk '{print $2}' | xargs chrt $2 -p $3
+ }
+
+ chrtirq "irq/20-fe00b880" -f 50
+ chrtirq "irq/66-xhci_hcd" -f 1
+ chrtirq "irq/50-VCHIQ do" -o 0
+ chrtirq "irq/27-DMA IRQ" -f 50
+ chrtirq "irq/51-mmc1" -o 0
+ chrtirq "irq/51-mmc0" -o 0
+ chrtirq "irq/51-s-mmc0" -o 0
+ chrtirq "irq/64-v3d" -o 0
+ chrtirq "irq/24-vc4 hvs" -o 0
+ chrtirq "irq/42-vc4 hdmi" -o 0
+ chrtirq "irq/43-vc4 hdmi" -o 0
+ chrtirq "irq/39-vc4 hdmi" -o 0
+ chrtirq "irq/39-s-vc4 hd" -o 0
+ chrtirq "irq/38-vc4 hdmi" -o 0
+ chrtirq "irq/38-s-vc4 hd" -o 0
+ chrtirq "irq/29-DMA IRQ" -f 50
+ chrtirq "irq/48-vc4 hdmi" -o 0
+ chrtirq "irq/49-vc4 hdmi" -o 0
+ chrtirq "irq/45-vc4 hdmi" -o 0
+ chrtirq "irq/45-s-vc4 hd" -o 0
+ chrtirq "irq/44-vc4 hdmi" -o 0
+ chrtirq "irq/44-s-vc4 hd" -o 0
+ chrtirq "irq/30-DMA IRQ" -f 50
+ chrtirq "irq/19-fe004000" -f 50
+ chrtirq "irq/34-vc4 crtc" -o 0
+ chrtirq "irq/35-vc4 crtc" -o 0
+ chrtirq "irq/36-vc4 crtc" -o 0
+ chrtirq "irq/35-vc4 crtc" -o 0
+ chrtirq "irq/37-vc4 crtc" -o 0
+ chrtirq "irq/23-uart-pl0" -o 0
+ chrtirq "irq/57-eth0" -f 10
+ chrtirq "irq/58-eth0" -f 10
+
# We have systemd configured to handle restarting, so just exec.
export PATH="${PATH}:/home/pi/robot_code"
rm -rf /dev/shm/aos
diff --git a/aos/util/BUILD b/aos/util/BUILD
index 0314069..8df16e2 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -257,6 +257,15 @@
],
)
+cc_library(
+ name = "crc32",
+ srcs = ["crc32.cc"],
+ hdrs = ["crc32.h"],
+ deps = [
+ "@com_google_absl//absl/types:span",
+ ],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
diff --git a/aos/events/logging/crc32.cc b/aos/util/crc32.cc
similarity index 95%
rename from aos/events/logging/crc32.cc
rename to aos/util/crc32.cc
index cd9d9fb..7f13f30 100644
--- a/aos/events/logging/crc32.cc
+++ b/aos/util/crc32.cc
@@ -1,4 +1,4 @@
-#include "aos/events/logging/crc32.h"
+#include "aos/util/crc32.h"
namespace aos {
@@ -53,9 +53,8 @@
return AccumulateCrc32(data, std::nullopt);
}
-uint32_t AccumulateCrc32(
- const absl::Span<uint8_t> data,
- std::optional<uint32_t> current_checksum) {
+uint32_t AccumulateCrc32(const absl::Span<uint8_t> data,
+ std::optional<uint32_t> current_checksum) {
uint32_t crc =
current_checksum.has_value() ? current_checksum.value() : 0xFF'FF'FF'FF;
for (const uint8_t n : data) {
diff --git a/aos/events/logging/crc32.h b/aos/util/crc32.h
similarity index 100%
rename from aos/events/logging/crc32.h
rename to aos/util/crc32.h
diff --git a/frc971/analysis/in_process_plotter.cc b/frc971/analysis/in_process_plotter.cc
index 0eaf719..cd52b1c 100644
--- a/frc971/analysis/in_process_plotter.cc
+++ b/frc971/analysis/in_process_plotter.cc
@@ -15,7 +15,7 @@
event_loop_factory_(&config_.message()),
event_loop_(event_loop_factory_.MakeEventLoop("plotter")),
plot_sender_(event_loop_->MakeSender<Plot>("/analysis")),
- web_proxy_(event_loop_.get(), -1),
+ web_proxy_(event_loop_.get(), aos::web_proxy::StoreHistory::kYes, -1),
builder_(plot_sender_.MakeBuilder()) {
web_proxy_.SetDataPath(kDataPath);
event_loop_->SkipTimingReport();
diff --git a/frc971/analysis/live_web_plotter_demo.sh b/frc971/analysis/live_web_plotter_demo.sh
index 4c4c9c4..45f3b92 100755
--- a/frc971/analysis/live_web_plotter_demo.sh
+++ b/frc971/analysis/live_web_plotter_demo.sh
@@ -1 +1 @@
-./aos/network/web_proxy_main --config=aos/network/www/test_config.json --data_dir=frc971/analysis
+./aos/network/web_proxy_main --config=aos/network/www/test_config.json --data_dir=frc971/analysis "$@"
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index cad3221..df6c0f6 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -20,6 +20,7 @@
kalman_q_vel,
kalman_q_voltage,
kalman_r_position,
+ radius = None,
dt=0.00505):
"""Constructs an AngularSystemParams object.
@@ -38,6 +39,7 @@
self.kalman_q_vel = kalman_q_vel
self.kalman_q_voltage = kalman_q_voltage
self.kalman_r_position = kalman_r_position
+ self.radius = radius
self.dt = dt
@@ -80,11 +82,17 @@
glog.debug('Controllability of %d',
numpy.linalg.matrix_rank(controllability))
glog.debug('J: %f', self.J)
- glog.debug('Stall torque: %f', self.motor.stall_torque / self.G)
- glog.debug('Stall acceleration: %f',
+ glog.debug('Stall torque: %f (N m)', self.motor.stall_torque / self.G)
+ if self.params.radius is not None:
+ glog.debug('Stall force: %f (N)',
+ self.motor.stall_torque / self.G / self.params.radius)
+ glog.debug('Stall force: %f (lbf)',
+ self.motor.stall_torque / self.G / self.params.radius * 0.224809)
+
+ glog.debug('Stall acceleration: %f (rad/s^2)',
self.motor.stall_torque / self.G / self.J)
- glog.debug('Free speed is %f',
+ glog.debug('Free speed is %f (rad/s)',
-self.B_continuous[1, 0] / self.A_continuous[1, 1] * 12.0)
self.Q = numpy.matrix([[(1.0 / (self.params.q_pos**2.0)), 0.0],
diff --git a/frc971/imu/ADIS16505.cc b/frc971/imu/ADIS16505.cc
index 4c31cab..bd65e4d 100644
--- a/frc971/imu/ADIS16505.cc
+++ b/frc971/imu/ADIS16505.cc
@@ -377,7 +377,12 @@
pwm_set_gpio_level(RATE_PWM, rate_level);
// 0 to 360
- uint16_t heading_level = (int16_t)(fmod(yaw, 360) / 360.0 * PWM_TOP);
+ double wrapped_heading = fmod(yaw, 360);
+ if (wrapped_heading < 0) {
+ wrapped_heading = wrapped_heading + 360;
+ }
+
+ uint16_t heading_level = (int16_t)(wrapped_heading / 360.0 * PWM_TOP);
pwm_set_gpio_level(HEADING_PWM, heading_level);
}
diff --git a/frc971/raspi/rootfs/README.md b/frc971/raspi/rootfs/README.md
index a9c7251..f8d9971 100644
--- a/frc971/raspi/rootfs/README.md
+++ b/frc971/raspi/rootfs/README.md
@@ -1,3 +1,5 @@
+# Creating an SD card to run the Raspberry Pis
+
This modifies a stock debian root filesystem to be able to operate as a vision
pi. It is not trying to be reproducible, but should be good enough for FRC
purposes.
@@ -5,15 +7,37 @@
The default hostname and IP is pi-971-1, 10.9.71.101.
Username pi, password raspberry.
-Download 2021-10-30-raspios-bullseye-armhf-lite.img (or any newer
-bullseye version, as a .zip file) from
-`https://www.raspberrypi.org/downloads/raspberry-pi-os/`, extract
-(unzip) the .img file, and edit `modify_rootfs.sh` to point to it.
+## Build the real-time kernel using `build_kernel.sh`
-Run modify_rootfs.sh to build the filesystem (you might need to hit
+- Checkout the real-time kernel source code, e.g.,
+ `cd CODE_DIR`
+ `git clone git@github.com:frc971/linux.git`
+ `git checkout frc971-5.10-pi4-rt branch`
+
+- Run `build_kernel.sh` to compile the real-time kernel
+ `cd ROOTFS_DIR` (where ROOTFS_DIR -> //frc971/raspi/rootfs)
+ `./build_kernel.sh CODE_DIR/linux kernel_5.10.tar.gz`
+
+## Download the Raspberry Pi OS
+
+Download the appropriate Raspberry Pi OS image, e.g.,
+`2022-01-28-raspios-bullseye-arm64-lite.img` (or any newer arm64
+bullseye version, as a .zip file) from
+`https://www.raspberrypi.org/downloads/raspberry-pi-os/`, and extract
+(unzip) the .img file.
+
+## Create our custom OS image using `modify_root.sh`
+
+- Edit `modify_rootfs.sh` to point to the kernel file (e.g.,
+`KERNEL=kernel_5.10.tar.gz`) and the Raspberry Pi OS image (e.g.,
+`IMAGE=2022-01-28-raspios-bullseye-arm64-lite.img`)
+
+- Run modify_rootfs.sh to build the filesystem (you might need to hit
return in a spot or two and will need sudo privileges to mount the
partition):
- * `modify_root.sh`
+ * `./modify_root.sh`
+
+## Write the file system to the SD card
VERY IMPORTANT NOTE: Before doing the next step, use `lsblk` to find
the device and make absolutely sure this isn't your hard drive or
diff --git a/frc971/raspi/rootfs/build_kernel.sh b/frc971/raspi/rootfs/build_kernel.sh
new file mode 100755
index 0000000..cd97902
--- /dev/null
+++ b/frc971/raspi/rootfs/build_kernel.sh
@@ -0,0 +1,48 @@
+#!/bin/bash
+# This script builds the kernel for the raspberry pi.
+#
+# git@github.com:frc971/linux.git
+#
+# We are using the frc971-5.10-pi4-rt branch
+#
+# Point it at the version of that repo you want to build, and it'll generate a
+# .tar.gz of the bits to install.
+
+
+set -ex
+
+echo $#
+if [[ $# -lt 2 ]];
+then
+ echo "Pass in the kernel directory and then the destination .tar.gz"
+ exit 1
+fi
+
+export OUTPUT="$(realpath $(dirname $2))/$(basename ${2})"
+export TMPDIR="${OUTPUT}_tmpdir"
+
+mkdir -p "${TMPDIR}"
+mkdir -p "${TMPDIR}/fat32/overlays"
+mkdir -p "${TMPDIR}/ext4"
+
+pushd "${1}"
+
+export KERNEL=kernel8
+
+make ARCH=arm64 CROSS_COMPILE=aarch64-linux-gnu- Image modules dtbs -j 60
+
+cp arch/arm64/boot/Image "${TMPDIR}/fat32/$KERNEL.img"
+cp arch/arm64/boot/dts/broadcom/*.dtb "${TMPDIR}/fat32/"
+cp arch/arm64/boot/dts/overlays/*.dtb* "${TMPDIR}/fat32/overlays/"
+cp arch/arm64/boot/dts/overlays/README "${TMPDIR}/fat32/overlays/"
+
+make ARCH=arm64 CROSS_COMPILE=aarch64-linux-gnu- INSTALL_MOD_PATH="${TMPDIR}/ext4" modules_install
+
+cd "${TMPDIR}"
+tar cvzf "${OUTPUT}" --owner=0 --group=0 "./"
+
+popd
+
+rm -rf "${TMPDIR}"
+
+echo "Kernel is now available at: ${OUTPUT}"
diff --git a/frc971/raspi/rootfs/frc971chrt.service b/frc971/raspi/rootfs/frc971chrt.service
new file mode 100644
index 0000000..cc68934
--- /dev/null
+++ b/frc971/raspi/rootfs/frc971chrt.service
@@ -0,0 +1,9 @@
+[Unit]
+Description=Configure IRQs
+
+[Service]
+Type=oneshot
+ExecStart=/home/pi/robot_code/chrt.sh
+
+[Install]
+WantedBy=multi-user.target
diff --git a/frc971/raspi/rootfs/make_sd.sh b/frc971/raspi/rootfs/make_sd.sh
index 81f7727..3bafacc 100755
--- a/frc971/raspi/rootfs/make_sd.sh
+++ b/frc971/raspi/rootfs/make_sd.sh
@@ -4,7 +4,7 @@
# Disk image to use for creating SD card
# NOTE: You MUST run modify_rootfs.sh on this image BEFORE running make_sd.sh
-ORIG_IMAGE="2021-10-30-raspios-bullseye-armhf-lite.img"
+ORIG_IMAGE="2022-01-28-raspios-bullseye-arm64-lite.img"
IMAGE=`echo ${ORIG_IMAGE} | sed s/.img/-frc-mods.img/`
DEVICE="/dev/sda"
@@ -22,7 +22,7 @@
sudo mount "${DEVICE}2" "${PARTITION}"
function target() {
- HOME=/root/ USER=root sudo proot -0 -q qemu-arm-static -w / -r "${PARTITION}" "$@"
+ HOME=/root/ USER=root sudo proot -0 -q qemu-aarch64-static -w / -r "${PARTITION}" "$@"
}
if [ "${1}" == "" ]; then
@@ -32,15 +32,15 @@
target /root/bin/change_hostname.sh "${1}"
fi
-echo "Starting a shell for any manual configuration"
-target /bin/bash --rcfile /root/.bashrc
-
# Put a timestamp on when this card got created and by whom
TIMESTAMP_FILE="${PARTITION}/home/pi/.DiskFlashedDate.txt"
date > "${TIMESTAMP_FILE}"
git rev-parse HEAD >> "${TIMESTAMP_FILE}"
whoami >> "${TIMESTAMP_FILE}"
+echo "Starting a shell for any manual configuration"
+target /bin/bash --rcfile /root/.bashrc
+
# Found I had to do a lazy force unmount ("-l" flag) to make it work reliably
sudo umount -l "${PARTITION}"
rmdir "${PARTITION}"
diff --git a/frc971/raspi/rootfs/modify_rootfs.sh b/frc971/raspi/rootfs/modify_rootfs.sh
index a340c64..0020f35 100755
--- a/frc971/raspi/rootfs/modify_rootfs.sh
+++ b/frc971/raspi/rootfs/modify_rootfs.sh
@@ -3,16 +3,18 @@
set -xe
# Full path to Raspberry Pi Bullseye disk image
-IMAGE="2021-10-30-raspios-bullseye-armhf-lite.img"
+IMAGE="2022-01-28-raspios-bullseye-arm64-lite.img"
+# Kernel built with build_kernel.sh
+KERNEL="kernel_5.10.tar.gz"
BOOT_PARTITION="${IMAGE}.boot_partition"
PARTITION="${IMAGE}.partition"
function target() {
- HOME=/root/ USER=root sudo proot -0 -q qemu-arm-static -w / -r "${PARTITION}" "$@"
+ HOME=/root/ USER=root sudo proot -0 -q qemu-aarch64-static -w / -r "${PARTITION}" "$@"
}
function user_pi_target() {
- USER=root sudo proot -0 -q qemu-arm-static -w / -r "${PARTITION}" sudo -h 127.0.0.1 -u pi "$@"
+ USER=root sudo proot -0 -q qemu-aarch64-static -w / -r "${PARTITION}" sudo -h 127.0.0.1 -u pi "$@"
}
@@ -37,6 +39,8 @@
# For now, disable the new libcamera driver in favor of legacy ones
sudo sed -i s/^camera_auto_detect=1/#camera_auto_detect=1/ "${BOOT_PARTITION}/config.txt"
+sudo tar -zxvf "${KERNEL}" --strip-components 2 -C ${BOOT_PARTITION}/ ./fat32
+
# Seeing a race condition with umount, so doing lazy umount
sudo umount -l "${BOOT_PARTITION}"
rmdir "${BOOT_PARTITION}"
@@ -82,6 +86,7 @@
sudo cp logind.conf "${PARTITION}/etc/systemd/logind.conf"
sudo cp change_hostname.sh "${PARTITION}/tmp/change_hostname.sh"
sudo cp frc971.service "${PARTITION}/etc/systemd/system/frc971.service"
+sudo cp frc971chrt.service "${PARTITION}/etc/systemd/system/frc971chrt.service"
sudo cp rt.conf "${PARTITION}/etc/security/limits.d/rt.conf"
sudo cp usb-mount@.service "${PARTITION}/etc/systemd/system/usb-mount@.service"
sudo cp 99-usb-mount.rules "${PARTITION}/etc/udev/rules.d/99-usb-mount.rules"
@@ -89,6 +94,9 @@
target /bin/mkdir -p /home/pi/.ssh/
cat ~/.ssh/id_rsa.pub | target tee /home/pi/.ssh/authorized_keys
+sudo rm -rf "${PARTITION}/lib/modules/"*
+sudo tar -zxvf "${KERNEL}" --strip-components 4 -C "${PARTITION}/lib/modules/" ./ext4/lib/modules/
+
# Downloads and installs our target libraries
target /bin/bash /tmp/target_configure.sh
diff --git a/frc971/raspi/rootfs/target_configure.sh b/frc971/raspi/rootfs/target_configure.sh
index 3fd4d40..db3fb36 100755
--- a/frc971/raspi/rootfs/target_configure.sh
+++ b/frc971/raspi/rootfs/target_configure.sh
@@ -17,7 +17,6 @@
apt-get install -y vim-nox \
git \
- python3-pip \
cpufrequtils \
libopencv-calib3d4.5 \
libopencv-contrib4.5 \
@@ -37,7 +36,6 @@
libopencv-videoio4.5 \
libopencv-videostab4.5 \
libopencv-viz4.5 \
- python3-opencv \
libnice10 \
pmount \
libnice-dev \
@@ -73,6 +71,7 @@
systemctl enable ssh.service
systemctl enable frc971.service
+systemctl enable frc971chrt.service
# Default us to pi-971-1
/root/bin/change_hostname.sh pi-971-1
diff --git a/frc971/wpilib/imu.fbs b/frc971/wpilib/imu.fbs
index abd01e2..565e3a4 100644
--- a/frc971/wpilib/imu.fbs
+++ b/frc971/wpilib/imu.fbs
@@ -45,6 +45,11 @@
// Operation section for more details on conditions that may cause this bit to
// be set to 1.
data_path_overrun:bool (id: 6);
+
+ // True indicates that the Raspberry Pi Pico recieved a packet
+ // from the imu that had a bad checksum but still sent a message
+ // containing a timestamp and encoder values.
+ checksum_mismatch:bool (id:7);
}
// Values returned from an IMU.
@@ -87,6 +92,24 @@
// converted from fpga_timestamp.
monotonic_timestamp_ns:long (id: 12);
+ // The timestamp when the values were captured by the Raspberry Pi Pico.
+ // This has microsecond precision.
+ pico_timestamp_us:int (id:20);
+
+ // The number of this reading produced from a 16-bit counter.
+ data_counter:int (id:19);
+
+ // The number of messages recieved by the Raspberry Pi that had bad checksums
+ failed_checksums:int (id:21);
+ // True if this packet has a bad checksum
+ // from the Raspberry Pi Pico to the Raspberry Pi.
+ checksum_failed:bool (id:22);
+
+ // The position of the left drivetrain encoder in meters
+ left_encoder:double (id: 17);
+ // The position of the right drivetrain encoder in meters
+ right_encoder:double (id: 18);
+
// For an ADIS16470, the DIAG_STAT value immediately after reset.
start_diag_stat:ADIS16470DiagStat (id: 13);
// For an ADIS16470, the DIAG_STAT value after the initial sensor self test we
diff --git a/tools/bazel b/tools/bazel
index e72b44e..da9008b 100755
--- a/tools/bazel
+++ b/tools/bazel
@@ -24,7 +24,7 @@
exec "${BAZEL_OVERRIDE}" "$@"
fi
-readonly VERSION="4.2.2"
+readonly VERSION="5.0.0"
readonly DOWNLOAD_DIR="${HOME}/.cache/bazel"
# Directory to unpack bazel into. This must change whenever bazel changes.
diff --git a/tools/ci/buildkite.yaml b/tools/ci/buildkite.yaml
index c4af4a4..e20c523 100644
--- a/tools/ci/buildkite.yaml
+++ b/tools/ci/buildkite.yaml
@@ -1,5 +1,5 @@
env:
- STARTUP: --max_idle_secs=0 --watchfs
+ STARTUP: --max_idle_secs=0
COMMON: -c opt --stamp=no --curses=yes --symlink_prefix=/ --disk_cache=~/.cache/bazel/disk_cache/ --repo_env=FRC971_RUNNING_IN_CI=1
TARGETS: //... @com_github_google_glog//... @com_google_ceres_solver//... @com_github_rawrtc_rawrtc//... @com_google_googletest//...
M4F_TARGETS: //...
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 0b05d10..ec5bf40 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -28,8 +28,8 @@
: event_loop_(event_loop),
pi_(pi),
pi_number_(aos::network::ParsePiNumber(pi)),
- prev_rvec_(Eigen::Vector3d::Zero()),
- prev_tvec_(Eigen::Vector3d::Zero()),
+ H_camera_board_(Eigen::Affine3d()),
+ prev_H_camera_board_(Eigen::Affine3d()),
charuco_extractor_(
event_loop, pi,
[this](cv::Mat rgb_image,
@@ -67,16 +67,15 @@
// Calibration calculates rotation and translation delta from last image
// captured to automatically capture next image
- Eigen::Matrix3d r_aff =
- Eigen::AngleAxisd(rvec_eigen.norm(), rvec_eigen / rvec_eigen.norm())
- .toRotationMatrix();
- Eigen::Matrix3d prev_r_aff =
- Eigen::AngleAxisd(prev_rvec_.norm(), prev_rvec_ / prev_rvec_.norm())
- .toRotationMatrix();
+ Eigen::Affine3d H_board_camera =
+ Eigen::Translation3d(tvec_eigen) *
+ Eigen::AngleAxisd(rvec_eigen.norm(), rvec_eigen / rvec_eigen.norm());
+ H_camera_board_ = H_board_camera.inverse();
+ Eigen::Affine3d H_delta = H_board_camera * prev_H_camera_board_;
- Eigen::AngleAxisd delta_r = Eigen::AngleAxisd(r_aff * prev_r_aff.inverse());
+ Eigen::AngleAxisd delta_r = Eigen::AngleAxisd(H_delta.rotation());
- Eigen::Vector3d delta_t = tvec_eigen - prev_tvec_;
+ Eigen::Vector3d delta_t = H_delta.translation();
double r_norm = std::abs(delta_r.angle());
double t_norm = delta_t.norm();
@@ -84,8 +83,7 @@
int keystroke = cv::waitKey(1);
if (r_norm > kDeltaRThreshold || t_norm > kDeltaTThreshold) {
if (valid) {
- prev_rvec_ = rvec_eigen;
- prev_tvec_ = tvec_eigen;
+ prev_H_camera_board_ = H_camera_board_;
all_charuco_ids_.emplace_back(std::move(charuco_ids));
all_charuco_corners_.emplace_back(std::move(charuco_corners));
@@ -194,8 +192,8 @@
std::vector<std::vector<int>> all_charuco_ids_;
std::vector<std::vector<cv::Point2f>> all_charuco_corners_;
- Eigen::Vector3d prev_rvec_;
- Eigen::Vector3d prev_tvec_;
+ Eigen::Affine3d H_camera_board_;
+ Eigen::Affine3d prev_H_camera_board_;
CharucoExtractor charuco_extractor_;
};
diff --git a/y2020/vision/calibration_accumulator.cc b/y2020/vision/calibration_accumulator.cc
index e77c74b..9f550c5 100644
--- a/y2020/vision/calibration_accumulator.cc
+++ b/y2020/vision/calibration_accumulator.cc
@@ -22,10 +22,15 @@
using aos::monotonic_clock;
namespace chrono = std::chrono;
+constexpr double kG = 9.807;
+
void CalibrationData::AddCameraPose(
distributed_clock::time_point distributed_now, Eigen::Vector3d rvec,
Eigen::Vector3d tvec) {
- rot_trans_points_.emplace_back(distributed_now, std::make_pair(rvec, tvec));
+ // Always start with IMU reading...
+ if (!imu_points_.empty() && imu_points_[0].first < distributed_now) {
+ rot_trans_points_.emplace_back(distributed_now, std::make_pair(rvec, tvec));
+ }
}
void CalibrationData::AddImu(distributed_clock::time_point distributed_now,
@@ -167,7 +172,7 @@
data_->AddImu(imu_factory_->ToDistributedClock(monotonic_clock::time_point(
chrono::nanoseconds(imu->monotonic_timestamp_ns()))),
- gyro, accel);
+ gyro, accel * kG);
}
} // namespace vision
diff --git a/y2020/vision/calibration_accumulator.h b/y2020/vision/calibration_accumulator.h
index 0f1bff7..7bff9f0 100644
--- a/y2020/vision/calibration_accumulator.h
+++ b/y2020/vision/calibration_accumulator.h
@@ -13,11 +13,17 @@
namespace frc971 {
namespace vision {
+// This class provides an interface for an application to be notified of all
+// camera and IMU samples in order with the correct timestamps.
class CalibrationDataObserver {
public:
+ // Observes a camera sample at the corresponding time t, and with the
+ // corresponding rotation and translation vectors rt.
virtual void UpdateCamera(aos::distributed_clock::time_point t,
std::pair<Eigen::Vector3d, Eigen::Vector3d> rt) = 0;
+ // Observes an IMU sample at the corresponding time t, and with the
+ // corresponding angular velocity and linear acceleration vectors wa.
virtual void UpdateIMU(aos::distributed_clock::time_point t,
std::pair<Eigen::Vector3d, Eigen::Vector3d> wa) = 0;
};
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index c7ef752..3216c23 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -29,6 +29,8 @@
using aos::distributed_clock;
using aos::monotonic_clock;
+constexpr double kGravity = 9.8;
+
// The basic ideas here are taken from Kalibr.
// (https://github.com/ethz-asl/kalibr), but adapted to work with AOS, and to be
// simpler.
@@ -61,48 +63,95 @@
template <typename Scalar>
class CeresPoseFilter : public CalibrationDataObserver {
public:
+ typedef Eigen::Transform<Scalar, 3, Eigen::Affine> Affine3s;
+
CeresPoseFilter(Eigen::Quaternion<Scalar> initial_orientation,
Eigen::Quaternion<Scalar> imu_to_camera,
- Eigen::Matrix<Scalar, 3, 1> imu_bias)
+ Eigen::Matrix<Scalar, 3, 1> gyro_bias,
+ Eigen::Matrix<Scalar, 6, 1> initial_state,
+ Eigen::Quaternion<Scalar> board_to_world,
+ Eigen::Matrix<Scalar, 3, 1> imu_to_camera_translation,
+ Scalar gravity_scalar,
+ Eigen::Matrix<Scalar, 3, 1> accelerometer_bias)
: accel_(Eigen::Matrix<double, 3, 1>::Zero()),
omega_(Eigen::Matrix<double, 3, 1>::Zero()),
- imu_bias_(imu_bias),
+ imu_bias_(gyro_bias),
orientation_(initial_orientation),
- x_hat_(Eigen::Matrix<Scalar, 6, 1>::Zero()),
+ x_hat_(initial_state),
p_(Eigen::Matrix<Scalar, 6, 6>::Zero()),
- imu_to_camera_(imu_to_camera) {}
+ imu_to_camera_rotation_(imu_to_camera),
+ imu_to_camera_translation_(imu_to_camera_translation),
+ board_to_world_(board_to_world),
+ gravity_scalar_(gravity_scalar),
+ accelerometer_bias_(accelerometer_bias) {}
- virtual void ObserveCameraUpdate(distributed_clock::time_point /*t*/,
- Eigen::Vector3d /*board_to_camera_rotation*/,
- Eigen::Quaternion<Scalar> /*imu_to_world*/) {
- }
+ Scalar gravity_scalar() { return gravity_scalar_; }
+ virtual void ObserveCameraUpdate(
+ distributed_clock::time_point /*t*/,
+ Eigen::Vector3d /*board_to_camera_rotation*/,
+ Eigen::Quaternion<Scalar> /*imu_to_world_rotation*/,
+ Affine3s /*imu_to_world*/) {}
+
+ // Observes a camera measurement by applying a kalman filter correction and
+ // accumulating up the error associated with the step.
void UpdateCamera(distributed_clock::time_point t,
std::pair<Eigen::Vector3d, Eigen::Vector3d> rt) override {
Integrate(t);
- Eigen::Quaternion<Scalar> board_to_camera(
+ const Eigen::Quaternion<Scalar> board_to_camera_rotation(
frc971::controls::ToQuaternionFromRotationVector(rt.first)
.cast<Scalar>());
+ const Affine3s board_to_camera =
+ Eigen::Translation3d(rt.second).cast<Scalar>() *
+ board_to_camera_rotation;
+
+ const Affine3s imu_to_camera =
+ imu_to_camera_translation_ * imu_to_camera_rotation_;
// This converts us from (facing the board),
// x right, y up, z towards us -> x right, y away, z up.
// Confirmed to be right.
- Eigen::Quaternion<Scalar> board_to_world(
- Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()).cast<Scalar>());
// Want world -> imu rotation.
// world <- board <- camera <- imu.
- const Eigen::Quaternion<Scalar> imu_to_world =
- board_to_world * board_to_camera.inverse() * imu_to_camera_;
+ const Eigen::Quaternion<Scalar> imu_to_world_rotation =
+ board_to_world_ * board_to_camera_rotation.inverse() *
+ imu_to_camera_rotation_;
- const Eigen::Quaternion<Scalar> error(imu_to_world.inverse() *
+ const Affine3s imu_to_world =
+ board_to_world_ * board_to_camera.inverse() * imu_to_camera;
+
+ const Eigen::Matrix<Scalar, 3, 1> z =
+ imu_to_world * Eigen::Matrix<Scalar, 3, 1>::Zero();
+
+ Eigen::Matrix<Scalar, 3, 6> H = Eigen::Matrix<Scalar, 3, 6>::Zero();
+ H(0, 0) = static_cast<Scalar>(1.0);
+ H(1, 1) = static_cast<Scalar>(1.0);
+ H(2, 2) = static_cast<Scalar>(1.0);
+ const Eigen::Matrix<Scalar, 3, 1> y = z - H * x_hat_;
+
+ const Eigen::Matrix<double, 3, 3> R =
+ (::Eigen::DiagonalMatrix<double, 3>().diagonal() << ::std::pow(0.01, 2),
+ ::std::pow(0.01, 2), ::std::pow(0.01, 2))
+ .finished()
+ .asDiagonal();
+
+ const Eigen::Matrix<Scalar, 3, 3> S =
+ H * p_ * H.transpose() + R.cast<Scalar>();
+ const Eigen::Matrix<Scalar, 6, 3> K = p_ * H.transpose() * S.inverse();
+
+ x_hat_ += K * y;
+ p_ = (Eigen::Matrix<Scalar, 6, 6>::Identity() - K * H) * p_;
+
+ const Eigen::Quaternion<Scalar> error(imu_to_world_rotation.inverse() *
orientation());
errors_.emplace_back(
Eigen::Matrix<Scalar, 3, 1>(error.x(), error.y(), error.z()));
+ position_errors_.emplace_back(y);
- ObserveCameraUpdate(t, rt.first, imu_to_world);
+ ObserveCameraUpdate(t, rt.first, imu_to_world_rotation, imu_to_world);
}
virtual void ObserveIMUUpdate(
@@ -120,14 +169,16 @@
const Eigen::Quaternion<Scalar> &orientation() const { return orientation_; }
- std::vector<Eigen::Matrix<Scalar, 3, 1>> errors_;
-
- // Returns the angular errors for each camera sample.
size_t num_errors() const { return errors_.size(); }
Scalar errorx(size_t i) const { return errors_[i].x(); }
Scalar errory(size_t i) const { return errors_[i].y(); }
Scalar errorz(size_t i) const { return errors_[i].z(); }
+ size_t num_perrors() const { return position_errors_.size(); }
+ Scalar errorpx(size_t i) const { return position_errors_[i].x(); }
+ Scalar errorpy(size_t i) const { return position_errors_[i].y(); }
+ Scalar errorpz(size_t i) const { return position_errors_[i].z(); }
+
private:
Eigen::Matrix<Scalar, 46, 1> Pack(Eigen::Quaternion<Scalar> q,
Eigen::Matrix<Scalar, 6, 1> x_hat,
@@ -151,7 +202,7 @@
return std::make_tuple(q, x_hat, p);
}
- Eigen::Matrix<Scalar, 46, 1> Derivitive(
+ Eigen::Matrix<Scalar, 46, 1> Derivative(
const Eigen::Matrix<Scalar, 46, 1> &input) {
auto [q, x_hat, p] = UnPack(input);
@@ -160,25 +211,48 @@
omega_q.vec() = 0.5 * (omega_.cast<Scalar>() - imu_bias_);
Eigen::Matrix<Scalar, 4, 1> q_dot = (q * omega_q).coeffs();
- Eigen::Matrix<Scalar, 6, 1> x_hat_dot = Eigen::Matrix<Scalar, 6, 1>::Zero();
- x_hat_dot(0, 0) = x_hat(3, 0);
- x_hat_dot(1, 0) = x_hat(4, 0);
- x_hat_dot(2, 0) = x_hat(5, 0);
- x_hat_dot.template block<3, 1>(3, 0) = accel_.cast<Scalar>();
+ Eigen::Matrix<double, 6, 6> A = Eigen::Matrix<double, 6, 6>::Zero();
+ A(0, 3) = 1.0;
+ A(1, 4) = 1.0;
+ A(2, 5) = 1.0;
- Eigen::Matrix<Scalar, 6, 6> p_dot = Eigen::Matrix<Scalar, 6, 6>::Zero();
+ Eigen::Matrix<Scalar, 6, 1> x_hat_dot = A * x_hat;
+ x_hat_dot.template block<3, 1>(3, 0) =
+ orientation() * (accel_.cast<Scalar>() - accelerometer_bias_) -
+ Eigen::Vector3d(0, 0, kGravity).cast<Scalar>() * gravity_scalar_;
+
+ // Initialize the position noise to 0. If the solver is going to back-solve
+ // for the most likely starting position, let's just say that the noise is
+ // small.
+ constexpr double kPositionNoise = 0.0;
+ constexpr double kAccelerometerNoise = 2.3e-6 * 9.8;
+ constexpr double kIMUdt = 5.0e-4;
+ Eigen::Matrix<double, 6, 6> Q_dot(
+ (::Eigen::DiagonalMatrix<double, 6>().diagonal()
+ << ::std::pow(kPositionNoise, 2) / kIMUdt,
+ ::std::pow(kPositionNoise, 2) / kIMUdt,
+ ::std::pow(kPositionNoise, 2) / kIMUdt,
+ ::std::pow(kAccelerometerNoise, 2) / kIMUdt,
+ ::std::pow(kAccelerometerNoise, 2) / kIMUdt,
+ ::std::pow(kAccelerometerNoise, 2) / kIMUdt)
+ .finished()
+ .asDiagonal());
+ Eigen::Matrix<Scalar, 6, 6> p_dot = A.cast<Scalar>() * p +
+ p * A.transpose().cast<Scalar>() +
+ Q_dot.cast<Scalar>();
return Pack(Eigen::Quaternion<Scalar>(q_dot), x_hat_dot, p_dot);
}
virtual void ObserveIntegrated(distributed_clock::time_point /*t*/,
Eigen::Matrix<Scalar, 6, 1> /*x_hat*/,
- Eigen::Quaternion<Scalar> /*orientation*/) {}
+ Eigen::Quaternion<Scalar> /*orientation*/,
+ Eigen::Matrix<Scalar, 6, 6> /*p*/) {}
void Integrate(distributed_clock::time_point t) {
if (last_time_ != distributed_clock::min_time) {
Eigen::Matrix<Scalar, 46, 1> next = control_loops::RungeKutta(
- [this](auto r) { return Derivitive(r); },
+ [this](auto r) { return Derivative(r); },
Pack(orientation_, x_hat_, p_),
aos::time::DurationInSeconds(t - last_time_));
@@ -189,34 +263,42 @@
}
last_time_ = t;
- ObserveIntegrated(t, x_hat_, orientation_);
+ ObserveIntegrated(t, x_hat_, orientation_, p_);
}
Eigen::Matrix<double, 3, 1> accel_;
Eigen::Matrix<double, 3, 1> omega_;
Eigen::Matrix<Scalar, 3, 1> imu_bias_;
+ // IMU -> world quaternion
Eigen::Quaternion<Scalar> orientation_;
Eigen::Matrix<Scalar, 6, 1> x_hat_;
Eigen::Matrix<Scalar, 6, 6> p_;
distributed_clock::time_point last_time_ = distributed_clock::min_time;
- Eigen::Quaternion<Scalar> imu_to_camera_;
+ Eigen::Quaternion<Scalar> imu_to_camera_rotation_;
+ Eigen::Translation<Scalar, 3> imu_to_camera_translation_ =
+ Eigen::Translation3d(0, 0, 0).cast<Scalar>();
- // States outside the KF:
- // orientation quaternion
- //
+ Eigen::Quaternion<Scalar> board_to_world_;
+ Scalar gravity_scalar_;
+ Eigen::Matrix<Scalar, 3, 1> accelerometer_bias_;
// States:
// xyz position
// xyz velocity
//
// Inputs
// xyz accel
- // angular rates
//
// Measurement:
- // xyz position
- // orientation rotation vector
+ // xyz position from camera.
+ //
+ // Since the gyro is so good, we can just solve for the bias and initial
+ // position with the solver and see what it learns.
+
+ // Returns the angular errors for each camera sample.
+ std::vector<Eigen::Matrix<Scalar, 3, 1>> errors_;
+ std::vector<Eigen::Matrix<Scalar, 3, 1>> position_errors_;
};
// Subclass of the filter above which has plotting. This keeps debug code and
@@ -225,25 +307,48 @@
public:
PoseFilter(Eigen::Quaternion<double> initial_orientation,
Eigen::Quaternion<double> imu_to_camera,
- Eigen::Matrix<double, 3, 1> imu_bias)
- : CeresPoseFilter<double>(initial_orientation, imu_to_camera, imu_bias) {}
+ Eigen::Matrix<double, 3, 1> gyro_bias,
+ Eigen::Matrix<double, 6, 1> initial_state,
+ Eigen::Quaternion<double> board_to_world,
+ Eigen::Matrix<double, 3, 1> imu_to_camera_translation,
+ double gravity_scalar,
+ Eigen::Matrix<double, 3, 1> accelerometer_bias)
+ : CeresPoseFilter<double>(initial_orientation, imu_to_camera, gyro_bias,
+ initial_state, board_to_world,
+ imu_to_camera_translation, gravity_scalar,
+ accelerometer_bias) {}
void Plot() {
+ std::vector<double> rx;
+ std::vector<double> ry;
+ std::vector<double> rz;
std::vector<double> x;
std::vector<double> y;
std::vector<double> z;
+ std::vector<double> vx;
+ std::vector<double> vy;
+ std::vector<double> vz;
for (const Eigen::Quaternion<double> &q : orientations_) {
Eigen::Matrix<double, 3, 1> rotation_vector =
frc971::controls::ToRotationVectorFromQuaternion(q);
- x.emplace_back(rotation_vector(0, 0));
- y.emplace_back(rotation_vector(1, 0));
- z.emplace_back(rotation_vector(2, 0));
+ rx.emplace_back(rotation_vector(0, 0));
+ ry.emplace_back(rotation_vector(1, 0));
+ rz.emplace_back(rotation_vector(2, 0));
}
+ for (const Eigen::Matrix<double, 6, 1> &x_hat : x_hats_) {
+ x.emplace_back(x_hat(0));
+ y.emplace_back(x_hat(1));
+ z.emplace_back(x_hat(2));
+ vx.emplace_back(x_hat(3));
+ vy.emplace_back(x_hat(4));
+ vz.emplace_back(x_hat(5));
+ }
+
frc971::analysis::Plotter plotter;
plotter.AddFigure("position");
- plotter.AddLine(times_, x, "x_hat(0)");
- plotter.AddLine(times_, y, "x_hat(1)");
- plotter.AddLine(times_, z, "x_hat(2)");
+ plotter.AddLine(times_, rx, "x_hat(0)");
+ plotter.AddLine(times_, ry, "x_hat(1)");
+ plotter.AddLine(times_, rz, "x_hat(2)");
plotter.AddLine(ct, cx, "Camera x");
plotter.AddLine(ct, cy, "Camera y");
plotter.AddLine(ct, cz, "Camera z");
@@ -253,9 +358,9 @@
plotter.Publish();
plotter.AddFigure("error");
- plotter.AddLine(times_, x, "x_hat(0)");
- plotter.AddLine(times_, y, "x_hat(1)");
- plotter.AddLine(times_, z, "x_hat(2)");
+ plotter.AddLine(times_, rx, "x_hat(0)");
+ plotter.AddLine(times_, ry, "x_hat(1)");
+ plotter.AddLine(times_, rz, "x_hat(2)");
plotter.AddLine(ct, cerrx, "Camera error x");
plotter.AddLine(ct, cerry, "Camera error y");
plotter.AddLine(ct, cerrz, "Camera error z");
@@ -268,6 +373,9 @@
plotter.AddLine(imut, imu_x, "imu x");
plotter.AddLine(imut, imu_y, "imu y");
plotter.AddLine(imut, imu_z, "imu z");
+ plotter.AddLine(times_, rx, "rotation x");
+ plotter.AddLine(times_, ry, "rotation y");
+ plotter.AddLine(times_, rz, "rotation z");
plotter.Publish();
plotter.AddFigure("raw");
@@ -282,12 +390,27 @@
plotter.AddLine(ct, raw_cz, "Camera z");
plotter.Publish();
+ plotter.AddFigure("xyz vel");
+ plotter.AddLine(times_, x, "x");
+ plotter.AddLine(times_, y, "y");
+ plotter.AddLine(times_, z, "z");
+ plotter.AddLine(times_, vx, "vx");
+ plotter.AddLine(times_, vy, "vy");
+ plotter.AddLine(times_, vz, "vz");
+ plotter.AddLine(ct, camera_position_x, "Camera x");
+ plotter.AddLine(ct, camera_position_y, "Camera y");
+ plotter.AddLine(ct, camera_position_z, "Camera z");
+ plotter.Publish();
+
plotter.Spin();
}
void ObserveIntegrated(distributed_clock::time_point t,
Eigen::Matrix<double, 6, 1> x_hat,
- Eigen::Quaternion<double> orientation) override {
+ Eigen::Quaternion<double> orientation,
+ Eigen::Matrix<double, 6, 6> p) override {
+ VLOG(1) << t << " -> " << p;
+ VLOG(1) << t << " xhat -> " << x_hat.transpose();
times_.emplace_back(chrono::duration<double>(t.time_since_epoch()).count());
x_hats_.emplace_back(x_hat);
orientations_.emplace_back(orientation);
@@ -309,18 +432,19 @@
void ObserveCameraUpdate(distributed_clock::time_point t,
Eigen::Vector3d board_to_camera_rotation,
- Eigen::Quaternion<double> imu_to_world) override {
+ Eigen::Quaternion<double> imu_to_world_rotation,
+ Eigen::Affine3d imu_to_world) override {
raw_cx.emplace_back(board_to_camera_rotation(0, 0));
raw_cy.emplace_back(board_to_camera_rotation(1, 0));
raw_cz.emplace_back(board_to_camera_rotation(2, 0));
Eigen::Matrix<double, 3, 1> rotation_vector =
- frc971::controls::ToRotationVectorFromQuaternion(imu_to_world);
+ frc971::controls::ToRotationVectorFromQuaternion(imu_to_world_rotation);
ct.emplace_back(chrono::duration<double>(t.time_since_epoch()).count());
Eigen::Matrix<double, 3, 1> cerr =
frc971::controls::ToRotationVectorFromQuaternion(
- imu_to_world.inverse() * orientation());
+ imu_to_world_rotation.inverse() * orientation());
cx.emplace_back(rotation_vector(0, 0));
cy.emplace_back(rotation_vector(1, 0));
@@ -330,11 +454,20 @@
cerry.emplace_back(cerr(1, 0));
cerrz.emplace_back(cerr(2, 0));
- const Eigen::Vector3d world_gravity = imu_to_world * last_accel_;
+ const Eigen::Vector3d world_gravity =
+ imu_to_world_rotation * last_accel_ -
+ Eigen::Vector3d(0, 0, kGravity) * gravity_scalar();
+
+ const Eigen::Vector3d camera_position =
+ imu_to_world * Eigen::Vector3d::Zero();
world_gravity_x.emplace_back(world_gravity.x());
world_gravity_y.emplace_back(world_gravity.y());
world_gravity_z.emplace_back(world_gravity.z());
+
+ camera_position_x.emplace_back(camera_position.x());
+ camera_position_y.emplace_back(camera_position.y());
+ camera_position_z.emplace_back(camera_position.z());
}
std::vector<double> ct;
@@ -354,6 +487,9 @@
std::vector<double> imu_x;
std::vector<double> imu_y;
std::vector<double> imu_z;
+ std::vector<double> camera_position_x;
+ std::vector<double> camera_position_y;
+ std::vector<double> camera_position_z;
std::vector<double> imut;
std::vector<double> imu_ratex;
@@ -375,13 +511,29 @@
template <typename S>
bool operator()(const S *const q1, const S *const q2, const S *const v,
- S *residual) const {
+ const S *const p, const S *const btw, const S *const itc,
+ const S *const gravity_scalar_ptr,
+ const S *const accelerometer_bias_ptr, S *residual) const {
Eigen::Quaternion<S> initial_orientation(q1[3], q1[0], q1[1], q1[2]);
Eigen::Quaternion<S> mounting_orientation(q2[3], q2[0], q2[1], q2[2]);
- Eigen::Matrix<S, 3, 1> imu_bias(v[0], v[1], v[2]);
+ Eigen::Quaternion<S> board_to_world(btw[3], btw[0], btw[1], btw[2]);
+ Eigen::Matrix<S, 3, 1> gyro_bias(v[0], v[1], v[2]);
+ Eigen::Matrix<S, 6, 1> initial_state;
+ initial_state(0) = p[0];
+ initial_state(1) = p[1];
+ initial_state(2) = p[2];
+ initial_state(3) = p[3];
+ initial_state(4) = p[4];
+ initial_state(5) = p[5];
+ Eigen::Matrix<S, 3, 1> imu_to_camera_translation(itc[0], itc[1], itc[2]);
+ Eigen::Matrix<S, 3, 1> accelerometer_bias(accelerometer_bias_ptr[0],
+ accelerometer_bias_ptr[1],
+ accelerometer_bias_ptr[2]);
CeresPoseFilter<S> filter(initial_orientation, mounting_orientation,
- imu_bias);
+ gyro_bias, initial_state, board_to_world,
+ imu_to_camera_translation, *gravity_scalar_ptr,
+ accelerometer_bias);
data->ReviewData(&filter);
for (size_t i = 0; i < filter.num_errors(); ++i) {
@@ -390,6 +542,12 @@
residual[3 * i + 2] = filter.errorz(i);
}
+ for (size_t i = 0; i < filter.num_perrors(); ++i) {
+ residual[3 * filter.num_errors() + 3 * i + 0] = filter.errorpx(i);
+ residual[3 * filter.num_errors() + 3 * i + 1] = filter.errorpy(i);
+ residual[3 * filter.num_errors() + 3 * i + 2] = filter.errorpz(i);
+ }
+
return true;
}
};
@@ -437,17 +595,29 @@
LOG(INFO) << "Done with event_loop running";
// And now we have it, we can start processing it.
- Eigen::Quaternion<double> nominal_initial_orientation(
+ const Eigen::Quaternion<double> nominal_initial_orientation(
frc971::controls::ToQuaternionFromRotationVector(
Eigen::Vector3d(0.0, 0.0, M_PI)));
- Eigen::Quaternion<double> nominal_imu_to_camera(
+ const Eigen::Quaternion<double> nominal_imu_to_camera(
Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX()));
+ const Eigen::Quaternion<double> nominal_board_to_world(
+ Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX()));
- Eigen::Quaternion<double> initial_orientation =
- Eigen::Quaternion<double>::Identity();
- Eigen::Quaternion<double> imu_to_camera =
- Eigen::Quaternion<double>::Identity();
- Eigen::Vector3d imu_bias = Eigen::Vector3d::Zero();
+ Eigen::Quaternion<double> initial_orientation = nominal_initial_orientation;
+ // Eigen::Quaternion<double>::Identity();
+ Eigen::Quaternion<double> imu_to_camera = nominal_imu_to_camera;
+ // Eigen::Quaternion<double>::Identity();
+ Eigen::Quaternion<double> board_to_world = nominal_board_to_world;
+ // Eigen::Quaternion<double>::Identity();
+ Eigen::Vector3d gyro_bias = Eigen::Vector3d::Zero();
+ Eigen::Matrix<double, 6, 1> initial_state =
+ Eigen::Matrix<double, 6, 1>::Zero();
+ Eigen::Matrix<double, 3, 1> imu_to_camera_translation =
+ Eigen::Matrix<double, 3, 1>::Zero();
+
+ double gravity_scalar = 1.0;
+ Eigen::Matrix<double, 3, 1> accelerometer_bias =
+ Eigen::Matrix<double, 3, 1>::Zero();
{
ceres::Problem problem;
@@ -458,19 +628,28 @@
// auto-differentiation to obtain the derivative (jacobian).
ceres::CostFunction *cost_function =
- new ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 4, 4, 3>(
- new CostFunctor(&data), data.camera_samples_size() * 3);
- problem.AddResidualBlock(cost_function, nullptr,
- initial_orientation.coeffs().data(),
- imu_to_camera.coeffs().data(), imu_bias.data());
+ new ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 4, 4, 3, 6,
+ 4, 3, 1, 3>(
+ new CostFunctor(&data), data.camera_samples_size() * 6);
+ problem.AddResidualBlock(
+ cost_function, nullptr, initial_orientation.coeffs().data(),
+ imu_to_camera.coeffs().data(), gyro_bias.data(), initial_state.data(),
+ board_to_world.coeffs().data(), imu_to_camera_translation.data(),
+ &gravity_scalar, accelerometer_bias.data());
problem.SetParameterization(initial_orientation.coeffs().data(),
quaternion_local_parameterization);
problem.SetParameterization(imu_to_camera.coeffs().data(),
quaternion_local_parameterization);
+ problem.SetParameterization(board_to_world.coeffs().data(),
+ quaternion_local_parameterization);
for (int i = 0; i < 3; ++i) {
- problem.SetParameterLowerBound(imu_bias.data(), i, -0.05);
- problem.SetParameterUpperBound(imu_bias.data(), i, 0.05);
+ problem.SetParameterLowerBound(gyro_bias.data(), i, -0.05);
+ problem.SetParameterUpperBound(gyro_bias.data(), i, 0.05);
+ problem.SetParameterLowerBound(accelerometer_bias.data(), i, -0.05);
+ problem.SetParameterUpperBound(accelerometer_bias.data(), i, 0.05);
}
+ problem.SetParameterLowerBound(&gravity_scalar, 0, 0.95);
+ problem.SetParameterUpperBound(&gravity_scalar, 0, 1.05);
// Run the solver!
ceres::Solver::Options options;
@@ -497,12 +676,28 @@
<< frc971::controls::ToRotationVectorFromQuaternion(
imu_to_camera * nominal_imu_to_camera.inverse())
.transpose();
- LOG(INFO) << "imu_bias " << imu_bias.transpose();
+ LOG(INFO) << "gyro_bias " << gyro_bias.transpose();
+ LOG(INFO) << "board_to_world " << board_to_world.coeffs().transpose();
+ LOG(INFO) << "board_to_world(rotation) "
+ << frc971::controls::ToRotationVectorFromQuaternion(
+ board_to_world)
+ .transpose();
+ LOG(INFO) << "board_to_world delta "
+ << frc971::controls::ToRotationVectorFromQuaternion(
+ board_to_world * nominal_board_to_world.inverse())
+ .transpose();
+ LOG(INFO) << "imu_to_camera_translation "
+ << imu_to_camera_translation.transpose();
+ LOG(INFO) << "gravity " << kGravity * gravity_scalar;
+ LOG(INFO) << "accelerometer bias " << accelerometer_bias.transpose();
}
{
- PoseFilter filter(initial_orientation, imu_to_camera, imu_bias);
+ PoseFilter filter(initial_orientation, imu_to_camera, gyro_bias,
+ initial_state, board_to_world, imu_to_camera_translation,
+ gravity_scalar, accelerometer_bias);
data.ReviewData(&filter);
+ filter.Plot();
}
}
diff --git a/y2022/BUILD b/y2022/BUILD
index 67e8587..78cb86c 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -28,6 +28,7 @@
binaries = [
"//y2020/vision:calibration",
"//y2022/vision:viewer",
+ "//y2022/localizer:imu_main",
],
data = [
":config",
@@ -167,6 +168,7 @@
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//y2022/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2022/control_loops/superstructure/intake:intake_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
@@ -233,3 +235,10 @@
"//y2022/control_loops/superstructure:superstructure_status_fbs",
],
)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2022/__init__.py b/y2022/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2022/__init__.py
diff --git a/y2022/constants.cc b/y2022/constants.cc
index 98f6511..73e72f3 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -11,6 +11,7 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
#include "glog/logging.h"
+#include "y2022/control_loops/superstructure/intake/integral_intake_plant.h"
namespace y2022 {
namespace constants {
@@ -26,6 +27,37 @@
const Values *DoGetValuesForTeam(uint16_t team) {
Values *const r = new Values();
+ // TODO(Yash): Set constants
+ // Intake constants.
+ auto *const intake = &r->intake;
+
+ intake->zeroing_voltage = 3.0;
+ intake->operating_voltage = 12.0;
+ intake->zeroing_profile_params = {0.5, 3.0};
+ intake->default_profile_params = {6.0, 30.0};
+ intake->range = Values::kIntakeRange();
+ intake->make_integral_loop =
+ control_loops::superstructure::intake::MakeIntegralIntakeLoop;
+
+ // The number of samples in the moving average filter.
+ intake->zeroing_constants.average_filter_size = Values::kZeroingSampleSize;
+ // The distance that the absolute encoder needs to complete a full rotation.
+ intake->zeroing_constants.one_revolution_distance =
+ M_PI * 2.0 * constants::Values::kIntakeEncoderRatio();
+
+ // Threshold for deciding if we are moving. moving_buffer_size samples need to
+ // be within this distance of each other before we use the middle one to zero.
+ intake->zeroing_constants.zeroing_threshold = 0.0005;
+ // Buffer size for deciding if we are moving.
+ intake->zeroing_constants.moving_buffer_size = 20;
+
+ // Value between 0 and 1 indicating what fraction of one_revolution_distance
+ // it is acceptable for the offset to move.
+ intake->zeroing_constants.allowable_encoder_error = 0.9;
+
+ // Measured absolute position of the encoder when at zero.
+ intake->zeroing_constants.measured_absolute_position = 0.0;
+
switch (team) {
// A set of constants for tests.
case 1:
diff --git a/y2022/constants.h b/y2022/constants.h
index 050a363..92f3a48 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "y2022/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2022/control_loops/superstructure/intake/intake_plant.h"
namespace y2022 {
namespace constants {
@@ -29,9 +30,31 @@
constants::Values::kDrivetrainEncoderRatio() *
kDrivetrainEncoderCountsPerRevolution();
}
+
+ static double DrivetrainEncoderToMeters(int32_t in) {
+ return ((static_cast<double>(in) /
+ kDrivetrainEncoderCountsPerRevolution()) *
+ (2.0 * M_PI)) *
+ kDrivetrainEncoderRatio() *
+ control_loops::drivetrain::kWheelRadius;
+ }
+
static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
static constexpr double kRollerStatorCurrentLimit() { return 40.0; }
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
+ intake;
+
+ // TODO (Yash): Constants need to be tuned
+ static constexpr ::frc971::constants::Range kIntakeRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.5, // Back Hard
+ .upper_hard = 2.85 + 0.05, // Front Hard
+ .lower = -0.300, // Back Soft
+ .upper = 2.725 // Front Soft
+ };
+ }
// Climber
static constexpr double kClimberSupplyCurrentLimit() { return 60.0; }
diff --git a/y2022/control_loops/BUILD b/y2022/control_loops/BUILD
new file mode 100644
index 0000000..49bc419
--- /dev/null
+++ b/y2022/control_loops/BUILD
@@ -0,0 +1,7 @@
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2022:python_init"],
+)
diff --git a/y2022/control_loops/__init__.py b/y2022/control_loops/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2022/control_loops/__init__.py
diff --git a/y2022/control_loops/python/BUILD b/y2022/control_loops/python/BUILD
index e0b0daa..4049acc 100644
--- a/y2022/control_loops/python/BUILD
+++ b/y2022/control_loops/python/BUILD
@@ -48,10 +48,42 @@
],
)
+py_binary(
+ name = "intake",
+ srcs = [
+ "intake.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:angular_system",
+ "//frc971/control_loops/python:controls",
+ ],
+)
+
+py_binary(
+ name = "turret",
+ srcs = [
+ "turret.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:angular_system",
+ "//frc971/control_loops/python:controls",
+ ],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
- deps = ["//y2020/control_loops:python_init"],
+ deps = ["//y2022/control_loops:python_init"],
)
diff --git a/y2022/control_loops/python/intake.py b/y2022/control_loops/python/intake.py
new file mode 100644
index 0000000..e5030e5
--- /dev/null
+++ b/y2022/control_loops/python/intake.py
@@ -0,0 +1,55 @@
+#!/usr/bin/python3
+
+from aos.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import angular_system
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kIntake = angular_system.AngularSystemParams(
+ name='Intake',
+ motor=control_loop.Falcon(),
+ # TODO(Milo): Change gear ratios when we have all of them
+ G=0.02,
+ J=0.34,
+ q_pos=0.40,
+ q_vel=20.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05,
+ radius=13 * 0.0254)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
+ angular_system.PlotKick(kIntake, R)
+ angular_system.PlotMotion(kIntake, R)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the intake and integral intake.'
+ )
+ else:
+ namespaces = ['y2022', 'control_loops', 'superstructure', 'intake']
+ angular_system.WriteAngularSystem(kIntake, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2022/control_loops/python/turret.py b/y2022/control_loops/python/turret.py
new file mode 100644
index 0000000..83ba4b3
--- /dev/null
+++ b/y2022/control_loops/python/turret.py
@@ -0,0 +1,54 @@
+#!/usr/bin/python3
+
+from aos.util.trapezoid_profile import TrapezoidProfile
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import angular_system
+from frc971.control_loops.python import controls
+import numpy
+import sys
+from matplotlib import pylab
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+kTurret = angular_system.AngularSystemParams(
+ name='Turret',
+ motor=control_loop.Falcon(),
+ G=0.01,
+ J=2.0,
+ q_pos=0.40,
+ q_vel=20.0,
+ kalman_q_pos=0.12,
+ kalman_q_vel=2.0,
+ kalman_q_voltage=4.0,
+ kalman_r_position=0.05,
+ radius=24 * 0.0254)
+
+
+def main(argv):
+ if FLAGS.plot:
+ R = numpy.matrix([[numpy.pi], [0.0]])
+ angular_system.PlotKick(kTurret, R)
+ angular_system.PlotMotion(kTurret, R)
+
+ # Write the generated constants out to a file.
+ if len(argv) != 5:
+ glog.fatal(
+ 'Expected .h file name and .cc file name for the turret and integral turret.'
+ )
+ else:
+ namespaces = ['y2022', 'control_loops', 'superstructure', 'turret']
+ angular_system.WriteAngularSystem(kTurret, argv[1:3], argv[3:5],
+ namespaces)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2022/control_loops/superstructure/intake/BUILD b/y2022/control_loops/superstructure/intake/BUILD
new file mode 100644
index 0000000..f6e5be0
--- /dev/null
+++ b/y2022/control_loops/superstructure/intake/BUILD
@@ -0,0 +1,34 @@
+package(default_visibility = ["//y2022:__subpackages__"])
+
+genrule(
+ name = "genrule_intake",
+ outs = [
+ "intake_plant.h",
+ "intake_plant.cc",
+ "integral_intake_plant.h",
+ "integral_intake_plant.cc",
+ ],
+ cmd = "$(location //y2022/control_loops/python:intake) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2022/control_loops/python:intake",
+ ],
+)
+
+cc_library(
+ name = "intake_plants",
+ srcs = [
+ "intake_plant.cc",
+ "integral_intake_plant.cc",
+ ],
+ hdrs = [
+ "intake_plant.h",
+ "integral_intake_plant.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
diff --git a/y2022/localizer/BUILD b/y2022/localizer/BUILD
new file mode 100644
index 0000000..c2afa55
--- /dev/null
+++ b/y2022/localizer/BUILD
@@ -0,0 +1,32 @@
+cc_library(
+ name = "imu",
+ srcs = [
+ "imu.cc",
+ ],
+ hdrs = [
+ "imu.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:epoll",
+ "//aos/events:shm_event_loop",
+ "//aos/util:crc32",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//frc971/wpilib:imu_fbs",
+ "//y2022:constants",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/types:span",
+ ],
+)
+
+cc_binary(
+ name = "imu_main",
+ srcs = ["imu_main.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":imu",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/y2022/localizer/imu.cc b/y2022/localizer/imu.cc
new file mode 100644
index 0000000..db7ec8f
--- /dev/null
+++ b/y2022/localizer/imu.cc
@@ -0,0 +1,168 @@
+#include "y2022/localizer/imu.h"
+
+#include "aos/util/crc32.h"
+#include "glog/logging.h"
+#include "y2022/constants.h"
+
+namespace y2022::localizer {
+
+namespace {
+
+constexpr size_t kReadSize = 50;
+constexpr double kGyroScale = 1 / 655360.0 / 360.0 * (2 * M_PI);
+constexpr double kAccelScale = 1 / 26756268.0 / 9.80665;
+constexpr double kTempScale = 0.1;
+
+} // namespace
+
+Imu::Imu(aos::ShmEventLoop *event_loop)
+ : event_loop_(event_loop),
+ imu_sender_(
+ event_loop_->MakeSender<frc971::IMUValuesBatch>("/drivetrain")) {
+ event_loop->SetRuntimeRealtimePriority(30);
+ imu_fd_ = open("/dev/adis16505", O_RDONLY | O_NONBLOCK);
+ PCHECK(imu_fd_ != -1) << ": Failed to open SPI device for IMU.";
+ aos::internal::EPoll *epoll = event_loop_->epoll();
+ epoll->OnReadable(imu_fd_, [this]() {
+ uint8_t buf[kReadSize];
+ ssize_t read_len = read(imu_fd_, buf, kReadSize);
+ // TODO: Do we care about gracefully handling EAGAIN or anything else?
+ // This should only get called when there is data.
+ PCHECK(read_len != -1);
+ CHECK_EQ(read_len, static_cast<ssize_t>(kReadSize))
+ << ": Read incorrect number of bytes.";
+
+ auto sender = imu_sender_.MakeBuilder();
+
+ const flatbuffers::Offset<frc971::IMUValues> values_offset =
+ ProcessReading(sender.fbb(), absl::Span(buf, kReadSize));
+ const flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::IMUValues>>>
+ readings_offset = sender.fbb()->CreateVector(&values_offset, 1);
+ frc971::IMUValuesBatch::Builder batch_builder =
+ sender.MakeBuilder<frc971::IMUValuesBatch>();
+ batch_builder.add_readings(readings_offset);
+ imu_sender_.CheckOk(sender.Send(batch_builder.Finish()));
+ });
+}
+
+flatbuffers::Offset<frc971::IMUValues> Imu::ProcessReading(
+ flatbuffers::FlatBufferBuilder *fbb, const absl::Span<uint8_t> message) {
+ absl::Span<const uint8_t> buf = message;
+
+ uint64_t driver_timestamp;
+ memcpy(&driver_timestamp, buf.data(), sizeof(driver_timestamp));
+ buf = buf.subspan(8);
+
+ uint16_t diag_stat;
+ memcpy(&diag_stat, buf.data(), sizeof(diag_stat));
+ buf = buf.subspan(2);
+
+ double x_gyro = ConvertValue32(buf, kGyroScale);
+ buf = buf.subspan(4);
+ double y_gyro = ConvertValue32(buf, kGyroScale);
+ buf = buf.subspan(4);
+ double z_gyro = ConvertValue32(buf, kGyroScale);
+ buf = buf.subspan(4);
+ double x_accel = ConvertValue32(buf, kAccelScale);
+ buf = buf.subspan(4);
+ double y_accel = ConvertValue32(buf, kAccelScale);
+ buf = buf.subspan(4);
+ double z_accel = ConvertValue32(buf, kAccelScale);
+ buf = buf.subspan(4);
+ double temp = ConvertValue16(buf, kTempScale);
+ buf = buf.subspan(2);
+ uint16_t data_counter;
+ memcpy(&data_counter, buf.data(), sizeof(data_counter));
+ buf = buf.subspan(2);
+ uint32_t pico_timestamp;
+ memcpy(&pico_timestamp, buf.data(), sizeof(pico_timestamp));
+ buf = buf.subspan(4);
+ int16_t encoder1_count;
+ memcpy(&encoder1_count, buf.data(), sizeof(encoder1_count));
+ buf = buf.subspan(2);
+ int16_t encoder2_count;
+ memcpy(&encoder2_count, buf.data(), sizeof(encoder2_count));
+ buf = buf.subspan(2);
+ uint32_t checksum;
+ memcpy(&checksum, buf.data(), sizeof(checksum));
+ buf = buf.subspan(4);
+
+ CHECK(buf.empty()) << "Have leftover bytes: " << buf.size();
+
+ u_int32_t calculated_checksum = aos::ComputeCrc32(message.subspan(8, 38));
+
+ if (checksum != calculated_checksum) {
+ this->failed_checksums_++;
+ }
+
+ const auto diag_stat_offset = PackDiagStat(fbb, diag_stat);
+
+ frc971::IMUValues::Builder imu_builder(*fbb);
+
+ if (checksum == calculated_checksum) {
+ constexpr uint16_t kChecksumMismatch = 1 << 0;
+ bool imu_checksum_matched = !(diag_stat & kChecksumMismatch);
+
+ // data from the IMU packet
+ if (imu_checksum_matched) {
+ imu_builder.add_gyro_x(x_gyro);
+ imu_builder.add_gyro_y(y_gyro);
+ imu_builder.add_gyro_z(z_gyro);
+
+ imu_builder.add_accelerometer_x(x_accel);
+ imu_builder.add_accelerometer_y(y_accel);
+ imu_builder.add_accelerometer_z(z_accel);
+
+ imu_builder.add_temperature(temp);
+
+ imu_builder.add_data_counter(data_counter);
+ }
+
+ // extra data from the pico
+ imu_builder.add_pico_timestamp_us(pico_timestamp);
+ imu_builder.add_left_encoder(
+ constants::Values::DrivetrainEncoderToMeters(encoder1_count));
+ imu_builder.add_right_encoder(
+ constants::Values::DrivetrainEncoderToMeters(encoder2_count));
+ imu_builder.add_previous_reading_diag_stat(diag_stat_offset);
+ }
+
+ // extra data from us
+ imu_builder.add_monotonic_timestamp_ns(driver_timestamp);
+ imu_builder.add_failed_checksums(failed_checksums_);
+ imu_builder.add_checksum_failed(checksum != calculated_checksum);
+
+ return imu_builder.Finish();
+}
+
+flatbuffers::Offset<frc971::ADIS16470DiagStat> Imu::PackDiagStat(
+ flatbuffers::FlatBufferBuilder *fbb, uint16_t value) {
+ frc971::ADIS16470DiagStat::Builder diag_stat_builder(*fbb);
+ diag_stat_builder.add_clock_error(value & (1 << 7));
+ diag_stat_builder.add_memory_failure(value & (1 << 6));
+ diag_stat_builder.add_sensor_failure(value & (1 << 5));
+ diag_stat_builder.add_standby_mode(value & (1 << 4));
+ diag_stat_builder.add_spi_communication_error(value & (1 << 3));
+ diag_stat_builder.add_flash_memory_update_error(value & (1 << 2));
+ diag_stat_builder.add_data_path_overrun(value & (1 << 1));
+ diag_stat_builder.add_checksum_mismatch(value & (1 << 0));
+ return diag_stat_builder.Finish();
+}
+
+double Imu::ConvertValue32(absl::Span<const uint8_t> data,
+ double lsb_per_output) {
+ int32_t value;
+ memcpy(&value, data.data(), sizeof(value));
+ return static_cast<double>(value) * lsb_per_output;
+}
+
+double Imu::ConvertValue16(absl::Span<const uint8_t> data,
+ double lsb_per_output) {
+ int16_t value;
+ memcpy(&value, data.data(), sizeof(value));
+ return static_cast<double>(value) * lsb_per_output;
+}
+
+Imu::~Imu() { PCHECK(0 == close(imu_fd_)); }
+} // namespace y2022::localizer
diff --git a/y2022/localizer/imu.h b/y2022/localizer/imu.h
new file mode 100644
index 0000000..cd45710
--- /dev/null
+++ b/y2022/localizer/imu.h
@@ -0,0 +1,31 @@
+#ifndef Y2022_LOCALIZER_IMU_H_
+#define Y2022_LOCALIZER_IMU_H_
+#include "aos/events/shm_event_loop.h"
+#include "frc971/wpilib/imu_batch_generated.h"
+#include "y2022/constants.h"
+
+namespace y2022::localizer {
+
+// Reads IMU packets from the kernel driver which reads them over spi
+// from the Raspberry Pi Pico on the IMU board.
+class Imu {
+ public:
+ Imu(aos::ShmEventLoop *event_loop);
+ ~Imu();
+
+ private:
+ flatbuffers::Offset<frc971::ADIS16470DiagStat> PackDiagStat(
+ flatbuffers::FlatBufferBuilder *fbb, uint16_t value);
+ flatbuffers::Offset<frc971::IMUValues> ProcessReading(
+ flatbuffers::FlatBufferBuilder *fbb, absl::Span<uint8_t> buf);
+ double ConvertValue32(absl::Span<const uint8_t> data, double lsb_per_output);
+ double ConvertValue16(absl::Span<const uint8_t> data, double lsb_per_output);
+
+ aos::ShmEventLoop *event_loop_;
+ aos::Sender<frc971::IMUValuesBatch> imu_sender_;
+ int imu_fd_;
+
+ uint failed_checksums_ = 0;
+};
+} // namespace y2022::localizer
+#endif // Y2022_LOCALIZER_IMU_H_
diff --git a/y2022/localizer/imu_main.cc b/y2022/localizer/imu_main.cc
new file mode 100644
index 0000000..bba2dd7
--- /dev/null
+++ b/y2022/localizer/imu_main.cc
@@ -0,0 +1,19 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2022/localizer/imu.h"
+
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+ y2022::localizer::Imu imu(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
index a732762..ce257a9 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -9,26 +9,40 @@
#include "opencv2/features2d.hpp"
#include "opencv2/imgproc.hpp"
-DEFINE_uint64(green_delta, 50,
- "Required difference between green pixels vs. red and blue");
+DEFINE_uint64(red_delta, 100,
+ "Required difference between green pixels vs. red");
+DEFINE_uint64(blue_delta, 50,
+ "Required difference between green pixels vs. blue");
+
DEFINE_bool(use_outdoors, false,
"If true, change thresholds to handle outdoor illumination");
+DEFINE_uint64(outdoors_red_delta, 100,
+ "Difference between green pixels vs. red, when outdoors");
+DEFINE_uint64(outdoors_blue_delta, 15,
+ "Difference between green pixels vs. blue, when outdoors");
namespace y2022 {
namespace vision {
-cv::Mat BlobDetector::ThresholdImage(cv::Mat rgb_image) {
- cv::Mat binarized_image(cv::Size(rgb_image.cols, rgb_image.rows), CV_8UC1);
- for (int row = 0; row < rgb_image.rows; row++) {
- for (int col = 0; col < rgb_image.cols; col++) {
- cv::Vec3b pixel = rgb_image.at<cv::Vec3b>(row, col);
+cv::Mat BlobDetector::ThresholdImage(cv::Mat bgr_image) {
+ size_t red_delta = FLAGS_red_delta;
+ size_t blue_delta = FLAGS_blue_delta;
+
+ if (FLAGS_use_outdoors) {
+ red_delta = FLAGS_outdoors_red_delta;
+ red_delta = FLAGS_outdoors_blue_delta;
+ }
+
+ cv::Mat binarized_image(cv::Size(bgr_image.cols, bgr_image.rows), CV_8UC1);
+ for (int row = 0; row < bgr_image.rows; row++) {
+ for (int col = 0; col < bgr_image.cols; col++) {
+ cv::Vec3b pixel = bgr_image.at<cv::Vec3b>(row, col);
uint8_t blue = pixel.val[0];
uint8_t green = pixel.val[1];
uint8_t red = pixel.val[2];
// Simple filter that looks for green pixels sufficiently brigher than
// red and blue
- if ((green > blue + FLAGS_green_delta) &&
- (green > red + FLAGS_green_delta)) {
+ if ((green > blue + blue_delta) && (green > red + red_delta)) {
binarized_image.at<uint8_t>(row, col) = 255;
} else {
binarized_image.at<uint8_t>(row, col) = 0;
@@ -143,15 +157,26 @@
}
double DistanceTo(cv::Point2d p) const {
- // Translate the point so that the circle orgin can be (0, 0)
- const auto p_prime = cv::Point2d(p.y - center.y, p.x - center.x);
+ const auto p_prime = TranslateToOrigin(p);
// Now, the distance is simply the difference between distance from the
// origin to p' and the radius.
return std::abs(cv::norm(p_prime) - radius);
}
- // Inverted because y-coordinates go backwards
- bool OnTopHalf(cv::Point2d p) const { return p.y <= center.y; }
+ bool InAngleRange(cv::Point2d p, double theta_min, double theta_max) const {
+ auto p_prime = TranslateToOrigin(p);
+ // Flip the y because y values go downwards.
+ p_prime.y *= -1;
+ const double theta = std::atan2(p_prime.y, p_prime.x);
+ return (theta >= theta_min && theta <= theta_max);
+ }
+
+ private:
+ // Translate the point on the circle
+ // as if the circle's center is the origin (0,0)
+ cv::Point2d TranslateToOrigin(cv::Point2d p) const {
+ return cv::Point2d(p.x - center.x, p.y - center.y);
+ }
};
} // namespace
@@ -169,24 +194,24 @@
// when the camera is the farthest from the target, at the field corner.
// We can solve for the pitch of the blob:
// blob_pitch = atan((height_tape - height_camera) / depth) + camera_pitch
- // The triangle with the height of the tape above the camera and the camera
- // depth is similar to the one with the focal length in y pixels and the y
- // coordinate offset from the center of the image.
- // Therefore y_offset = focal_length_y * tan(blob_pitch), and
- // y = -(y_offset - offset_y)
+ // The triangle with the height of the tape above the camera and the
+ // camera depth is similar to the one with the focal length in y pixels
+ // and the y coordinate offset from the center of the image. Therefore
+ // y_offset = focal_length_y * tan(blob_pitch), and y = -(y_offset -
+ // offset_y)
constexpr int kMaxY = 400;
constexpr double kTapeAspectRatio = 5.0 / 2.0;
- constexpr double kAspectRatioThreshold = 1.5;
+ constexpr double kAspectRatioThreshold = 1.6;
constexpr double kMinArea = 10;
- constexpr size_t kMinPoints = 6;
+ constexpr size_t kMinNumPoints = 6;
// Remove all blobs that are at the bottom of the image, have a different
- // aspect ratio than the tape, or have too little area or points
- // TODO(milind): modify to take into account that blobs will be on the side.
+ // aspect ratio than the tape, or have too little area or points.
if ((stats_it->centroid.y <= kMaxY) &&
(std::abs(kTapeAspectRatio - stats_it->aspect_ratio) <
kAspectRatioThreshold) &&
- (stats_it->area >= kMinArea) && (stats_it->num_points >= kMinPoints)) {
+ (stats_it->area >= kMinArea) &&
+ (stats_it->num_points >= kMinNumPoints)) {
filtered_blobs.push_back(*blob_it);
filtered_stats.push_back(*stats_it);
}
@@ -196,6 +221,9 @@
// Threshold for mean distance from a blob centroid to a circle.
constexpr double kCircleDistanceThreshold = 5.0;
+ // We should only expect to see blobs between these angles on a circle.
+ constexpr double kMinBlobAngle = M_PI / 3;
+ constexpr double kMaxBlobAngle = M_PI - kMinBlobAngle;
std::vector<std::vector<cv::Point>> blob_circle;
std::vector<cv::Point2d> centroids;
@@ -230,16 +258,20 @@
continue;
}
- // Only try to fit points to this circle if all of these are on the top
- // half, like how the blobs should be
- if (circle->OnTopHalf(current_centroids[0]) &&
- circle->OnTopHalf(current_centroids[1]) &&
- circle->OnTopHalf(current_centroids[2])) {
+ // Only try to fit points to this circle if all of these are between
+ // certain angles.
+ if (circle->InAngleRange(current_centroids[0], kMinBlobAngle,
+ kMaxBlobAngle) &&
+ circle->InAngleRange(current_centroids[1], kMinBlobAngle,
+ kMaxBlobAngle) &&
+ circle->InAngleRange(current_centroids[2], kMinBlobAngle,
+ kMaxBlobAngle)) {
for (size_t m = 0; m < filtered_blobs.size(); m++) {
// Add this blob to the list if it is close to the circle, is on the
// top half, and isn't one of the other blobs
if ((m != i) && (m != j) && (m != k) &&
- circle->OnTopHalf(filtered_stats[m].centroid) &&
+ circle->InAngleRange(filtered_stats[m].centroid, kMinBlobAngle,
+ kMaxBlobAngle) &&
(circle->DistanceTo(filtered_stats[m].centroid) <
kCircleDistanceThreshold)) {
current_blobs.emplace_back(filtered_blobs[m]);
@@ -293,10 +325,10 @@
cv::circle(view_image, centroid, 3, cv::Scalar(255, 255, 0), cv::FILLED);
}
-void BlobDetector::ExtractBlobs(cv::Mat rgb_image,
+void BlobDetector::ExtractBlobs(cv::Mat bgr_image,
BlobDetector::BlobResult *blob_result) {
auto start = aos::monotonic_clock::now();
- blob_result->binarized_image = ThresholdImage(rgb_image);
+ blob_result->binarized_image = ThresholdImage(bgr_image);
blob_result->unfiltered_blobs = FindBlobs(blob_result->binarized_image);
blob_result->blob_stats = ComputeStats(blob_result->unfiltered_blobs);
auto filtered_pair =
diff --git a/y2022/vision/blob_detector.h b/y2022/vision/blob_detector.h
index 4077e2c..d263d32 100644
--- a/y2022/vision/blob_detector.h
+++ b/y2022/vision/blob_detector.h
@@ -28,7 +28,7 @@
// Given an image, threshold it to find "green" pixels
// Input: Color image
// Output: Grayscale (binarized) image with green pixels set to 255
- static cv::Mat ThresholdImage(cv::Mat rgb_image);
+ static cv::Mat ThresholdImage(cv::Mat bgr_image);
// Given binary image, extract blobs
static std::vector<std::vector<cv::Point>> FindBlobs(cv::Mat threshold_image);
@@ -52,7 +52,7 @@
const std::vector<std::vector<cv::Point>> &unfiltered_blobs,
const std::vector<BlobStats> &blob_stats, cv::Point centroid);
- static void ExtractBlobs(cv::Mat rgb_image, BlobResult *blob_result);
+ static void ExtractBlobs(cv::Mat bgr_image, BlobResult *blob_result);
};
} // namespace vision
} // namespace y2022
diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index db87cb0..abce18c 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -124,8 +124,8 @@
// that can be sent in a second.
std::this_thread::sleep_for(std::chrono::milliseconds(50));
LOG(INFO) << "Reading file " << file;
- cv::Mat rgb_image = cv::imread(file.c_str());
- ProcessImage(rgb_image);
+ cv::Mat bgr_image = cv::imread(file.c_str());
+ ProcessImage(bgr_image);
}
event_loop_->Exit();
return;
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index 6ee6144..c9c2aff 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -76,11 +76,11 @@
// Create color image:
cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
(void *)image->data()->data());
- cv::Mat rgb_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
- cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV);
+ cv::Mat bgr_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2RGB_YUYV);
if (!FLAGS_capture.empty()) {
- cv::imwrite(FLAGS_capture, rgb_image);
+ cv::imwrite(FLAGS_capture, bgr_image);
return false;
}
@@ -100,15 +100,15 @@
cv::imshow("blobs", ret_image);
}
- cv::imshow("image", rgb_image);
+ cv::imshow("image", bgr_image);
int keystroke = cv::waitKey(1);
if ((keystroke & 0xFF) == static_cast<int>('c')) {
// Convert again, to get clean image
- cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV);
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
std::stringstream name;
name << "capture-" << aos::realtime_clock::now() << ".png";
- cv::imwrite(name.str(), rgb_image);
+ cv::imwrite(name.str(), bgr_image);
LOG(INFO) << "Saved image file: " << name.str();
} else if ((keystroke & 0xFF) == static_cast<int>('q')) {
return false;
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 0ee1e92..673fa80 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -68,14 +68,6 @@
// DMA stuff and then removing the * 2.0 in *_translate.
// The low bit is direction.
-double drivetrain_translate(int32_t in) {
- return ((static_cast<double>(in) /
- Values::kDrivetrainEncoderCountsPerRevolution()) *
- (2.0 * M_PI)) *
- Values::kDrivetrainEncoderRatio() *
- control_loops::drivetrain::kWheelRadius;
-}
-
double drivetrain_velocity_translate(double in) {
return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
(2.0 * M_PI)) *
@@ -127,12 +119,14 @@
frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
drivetrain_builder.add_left_encoder(
- drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+ constants::Values::DrivetrainEncoderToMeters(
+ drivetrain_left_encoder_->GetRaw()));
drivetrain_builder.add_left_speed(
drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
drivetrain_builder.add_right_encoder(
- -drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+ -constants::Values::DrivetrainEncoderToMeters(
+ drivetrain_right_encoder_->GetRaw()));
drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
drivetrain_right_encoder_->GetPeriod()));
diff --git a/y2022/y2022_pi_template.json b/y2022/y2022_pi_template.json
index 823a7f5..07835a0 100644
--- a/y2022/y2022_pi_template.json
+++ b/y2022/y2022_pi_template.json
@@ -110,7 +110,8 @@
"type": "y2022.vision.TargetEstimate",
"source_node": "pi{{ NUM }}",
"frequency": 25,
- "num_senders": 2
+ "num_senders": 2,
+ "max_size": 20000
},
{
"name": "/logger/aos",