Merge "Split camera_reader into a lib and main"
diff --git a/.bazelrc b/.bazelrc
index 3b5ad1f..8290da4 100644
--- a/.bazelrc
+++ b/.bazelrc
@@ -17,12 +17,6 @@
# Use the malloc we want.
build --custom_malloc=//tools/cpp:malloc
-# Use our hermetic Python runtime.
-build --python_top=//tools/python:runtime
-build --noincompatible_use_python_toolchains
-build --noincompatible_py3_is_default
-build --noincompatible_py2_outputs_are_suffixed
-
# Shortcuts for selecting the target platform.
build:k8 --platforms=//tools/platforms:linux_x86
build:roborio --platforms=//tools/platforms:linux_roborio
diff --git a/WORKSPACE b/WORKSPACE
index 23c4580..a464a6d 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -125,6 +125,8 @@
"//tools/cpp:cc-toolchain-cortex-m4f",
# Find a good way to select between these two M4F toolchains.
#"//tools/cpp:cc-toolchain-cortex-m4f-k22",
+ "//tools/python:python_toolchain",
+ "//tools/go:noop_go_toolchain",
)
http_archive(
@@ -230,8 +232,9 @@
# TODO: add frc971.org URL
http_archive(
name = "rules_python",
- sha256 = "778197e26c5fbeb07ac2a2c5ae405b30f6cb7ad1f5510ea6fdac03bded96cc6f",
- url = "https://github.com/bazelbuild/rules_python/releases/download/0.2.0/rules_python-0.2.0.tar.gz",
+ sha256 = "895fa3b03898d7708eb50ed34dcfb71c07866433df6912a6ff4f4fb473048f99",
+ strip_prefix = "rules_python-2b1d6beb4d5d8f59d629597e30e9aa519182d9a9",
+ url = "https://github.com/bazelbuild/rules_python/archive/2b1d6beb4d5d8f59d629597e30e9aa519182d9a9.tar.gz",
)
new_local_repository(
@@ -871,3 +874,18 @@
name = "snappy",
path = "third_party/snappy",
)
+
+http_archive(
+ name = "io_bazel_rules_go",
+ sha256 = "2b1641428dff9018f9e85c0384f03ec6c10660d935b750e3fa1492a281a53b0f",
+ urls = [
+ "https://www.frc971.org/Build-Dependencies/rules_go-v0.29.0.zip",
+ "https://github.com/bazelbuild/rules_go/releases/download/v0.29.0/rules_go-v0.29.0.zip",
+ ],
+)
+
+load("@io_bazel_rules_go//go:deps.bzl", "go_register_toolchains", "go_rules_dependencies")
+
+go_rules_dependencies()
+
+go_register_toolchains(version = "1.17.1")
diff --git a/aos/BUILD b/aos/BUILD
index 9049e08..8a8a19a 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -409,6 +409,7 @@
deps = [
"//aos:macros",
"//aos/containers:resizeable_buffer",
+ "//aos/util:file",
"@com_github_google_flatbuffers//:flatbuffers",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/strings",
@@ -581,6 +582,7 @@
":json_to_flatbuffer",
":json_to_flatbuffer_fbs",
"//aos/testing:googletest",
+ "//aos/testing:tmpdir",
],
)
diff --git a/aos/configuration.cc b/aos/configuration.cc
index bc7182c..580c2e0 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -610,6 +610,9 @@
auto result = applications.insert(RecursiveCopyFlatBuffer(a));
if (!result.second) {
+ if (a->has_args()) {
+ result.first->mutable_message()->clear_args();
+ }
*result.first =
MergeFlatBuffers(*result.first, RecursiveCopyFlatBuffer(a));
}
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index ac2da4c..c81fa1e 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -593,7 +593,7 @@
}
// Like MakeSender, but returns an invalid sender if the given channel is
- // not readable on this node or does not exist.
+ // not sendable on this node or does not exist.
template <typename T>
Sender<T> TryMakeSender(const std::string_view channel_name) {
const Channel *channel = GetChannel<T>(channel_name);
diff --git a/aos/events/event_loop_event.h b/aos/events/event_loop_event.h
index 808a4fe..a86f670 100644
--- a/aos/events/event_loop_event.h
+++ b/aos/events/event_loop_event.h
@@ -32,7 +32,7 @@
}
void set_generation(size_t generation) { generation_ = generation; }
- virtual void HandleEvent() = 0;
+ virtual void HandleEvent() noexcept = 0;
private:
monotonic_clock::time_point event_time_ = monotonic_clock::max_time;
@@ -45,7 +45,7 @@
public:
EventHandler(T *t) : t_(t) {}
~EventHandler() override = default;
- void HandleEvent() override { t_->HandleEvent(); }
+ void HandleEvent() noexcept override { t_->HandleEvent(); }
private:
T *const t_;
diff --git a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc
index 30e293d..fca25d4 100644
--- a/aos/events/event_loop_param_test.cc
+++ b/aos/events/event_loop_param_test.cc
@@ -1186,14 +1186,14 @@
auto ender_timer = loop->AddTimer([&test_timer]() { test_timer->Disable(); });
monotonic_clock::time_point s = loop->monotonic_now();
- test_timer->Setup(s, ::std::chrono::milliseconds(70));
- ender_timer->Setup(s + ::std::chrono::milliseconds(200));
- EndEventLoop(loop.get(), ::std::chrono::milliseconds(350));
+ test_timer->Setup(s, ::std::chrono::milliseconds(500));
+ ender_timer->Setup(s + ::std::chrono::milliseconds(1250));
+ EndEventLoop(loop.get(), ::std::chrono::milliseconds(2000));
Run();
EXPECT_THAT(iteration_list,
- ::testing::ElementsAre(s, s + chrono::milliseconds(70),
- s + chrono::milliseconds(140)));
+ ::testing::ElementsAre(s, s + chrono::milliseconds(500),
+ s + chrono::milliseconds(1000)));
}
// Verify that a timer can disable itself.
diff --git a/aos/events/event_scheduler.cc b/aos/events/event_scheduler.cc
index 6f2c2c8..c06638c 100644
--- a/aos/events/event_scheduler.cc
+++ b/aos/events/event_scheduler.cc
@@ -8,8 +8,8 @@
namespace aos {
-EventScheduler::Token EventScheduler::Schedule(
- monotonic_clock::time_point time, ::std::function<void()> callback) {
+EventScheduler::Token EventScheduler::Schedule(monotonic_clock::time_point time,
+ Event *callback) {
return events_list_.emplace(time, callback);
}
@@ -58,9 +58,9 @@
CHECK_EQ(t.boot, boot_count_);
CHECK_EQ(t.time, iter->first) << ": Time is wrong on node " << node_index_;
- ::std::function<void()> callback = ::std::move(iter->second);
+ Event *callback = iter->second;
events_list_.erase(iter);
- callback();
+ callback->Handle();
converter_->ObserveTimePassed(scheduler_scheduler_->distributed_now());
}
diff --git a/aos/events/event_scheduler.h b/aos/events/event_scheduler.h
index cc70757..55c1cf8 100644
--- a/aos/events/event_scheduler.h
+++ b/aos/events/event_scheduler.h
@@ -85,8 +85,13 @@
class EventScheduler {
public:
- using ChannelType =
- std::multimap<monotonic_clock::time_point, std::function<void()>>;
+ class Event {
+ public:
+ virtual void Handle() noexcept = 0;
+ virtual ~Event() {}
+ };
+
+ using ChannelType = std::multimap<monotonic_clock::time_point, Event *>;
using Token = ChannelType::iterator;
EventScheduler(size_t node_index) : node_index_(node_index) {}
@@ -97,14 +102,11 @@
converter_ = converter;
}
- UUID boot_uuid() {
- return converter_->boot_uuid(node_index_, boot_count_);
- }
+ UUID boot_uuid() { return converter_->boot_uuid(node_index_, boot_count_); }
// Schedule an event with a callback function
// Returns an iterator to the event
- Token Schedule(monotonic_clock::time_point time,
- std::function<void()> callback);
+ Token Schedule(monotonic_clock::time_point time, Event *callback);
// Schedules a callback when the event scheduler starts.
void ScheduleOnRun(std::function<void()> callback) {
diff --git a/aos/events/logging/log_namer.h b/aos/events/logging/log_namer.h
index f1f1829..00e1856 100644
--- a/aos/events/logging/log_namer.h
+++ b/aos/events/logging/log_namer.h
@@ -106,8 +106,8 @@
monotonic_clock::time_point monotonic_start_time_ = monotonic_clock::min_time;
- const Node *const node_ = nullptr;
- const size_t node_index_ = 0;
+ const Node *node_ = nullptr;
+ size_t node_index_ = 0;
LogNamer *log_namer_;
UUID parts_uuid_ = UUID::Random();
size_t parts_index_ = 0;
diff --git a/aos/events/logging/log_reader.h b/aos/events/logging/log_reader.h
index e367f0c..5cccdeb 100644
--- a/aos/events/logging/log_reader.h
+++ b/aos/events/logging/log_reader.h
@@ -163,6 +163,14 @@
true) != nullptr;
}
+ template <typename T>
+ void MaybeRemapLoggedChannel(std::string_view name,
+ const Node *node = nullptr) {
+ if (HasChannel<T>(name, node)) {
+ RemapLoggedChannel<T>(name, node);
+ }
+ }
+
// Returns true if the channel exists on the node and was logged.
template <typename T>
bool HasLoggedChannel(std::string_view name, const Node *node = nullptr) {
diff --git a/aos/events/logging/logfile_sorting.cc b/aos/events/logging/logfile_sorting.cc
index 7eb17a0..7610295 100644
--- a/aos/events/logging/logfile_sorting.cc
+++ b/aos/events/logging/logfile_sorting.cc
@@ -15,6 +15,9 @@
#include "openssl/sha.h"
#include "sys/stat.h"
+DEFINE_bool(quiet_sorting, false,
+ "If true, sort with minimal messages about truncated files.");
+
namespace aos {
namespace logger {
namespace chrono = std::chrono;
@@ -295,12 +298,13 @@
// Map from a observed pair of boots to the associated timestamps.
// logger_node -> logger_node_boot_uuid -> destination_node index ->
- // destination_boot_uuid -> times.
+ // destination_boot_uuid -> list of all times from all parts.
absl::btree_map<
std::string,
absl::btree_map<
std::string,
- absl::btree_map<size_t, absl::btree_map<std::string, BootPairTimes>>>>
+ absl::btree_map<size_t, absl::btree_map<std::string,
+ std::vector<BootPairTimes>>>>>
boot_times;
// Map holding the log_event_uuid -> second map. The second map holds the
@@ -342,11 +346,13 @@
// resources, so close a big batch at once periodically.
part_readers.clear();
}
- part_readers.emplace_back(part);
+ part_readers.emplace_back(part, FLAGS_quiet_sorting);
std::optional<SizePrefixedFlatbufferVector<LogFileHeader>> log_header =
ReadHeader(&part_readers.back());
if (!log_header) {
- LOG(WARNING) << "Skipping " << part << " without a header";
+ if (!FLAGS_quiet_sorting) {
+ LOG(WARNING) << "Skipping " << part << " without a header";
+ }
corrupted.emplace_back(part);
continue;
}
@@ -461,7 +467,9 @@
std::optional<SizePrefixedFlatbufferVector<MessageHeader>> first_message =
ReadNthMessage(part, 0);
if (!first_message) {
- LOG(WARNING) << "Skipping " << part << " without any messages";
+ if (!FLAGS_quiet_sorting) {
+ LOG(WARNING) << "Skipping " << part << " without any messages";
+ }
corrupted.emplace_back(part);
continue;
}
@@ -571,7 +579,8 @@
std::string,
absl::btree_map<
size_t,
- absl::btree_map<std::string, BootPairTimes>>>())
+ absl::btree_map<std::string,
+ std::vector<BootPairTimes>>>>())
.first;
}
auto source_boot_times_it =
@@ -583,7 +592,8 @@
.emplace(
logger_boot_uuid,
absl::btree_map<
- size_t, absl::btree_map<std::string, BootPairTimes>>())
+ size_t, absl::btree_map<std::string,
+ std::vector<BootPairTimes>>>())
.first;
}
@@ -646,7 +656,8 @@
destination_boot_times_it =
source_boot_times_it->second
.emplace(node_index,
- absl::btree_map<std::string, BootPairTimes>())
+ absl::btree_map<std::string,
+ std::vector<BootPairTimes>>())
.first;
}
@@ -656,7 +667,17 @@
if (boot_times_it == destination_boot_times_it->second.end()) {
// We have a new boot UUID pairing. Copy over the data we have.
destination_boot_times_it->second.emplace(
- boot_uuid,
+ boot_uuid, std::vector<BootPairTimes>{BootPairTimes{
+ .oldest_remote_monotonic_timestamp =
+ oldest_remote_monotonic_timestamp,
+ .oldest_local_monotonic_timestamp =
+ oldest_local_monotonic_timestamp,
+ .oldest_remote_unreliable_monotonic_timestamp =
+ oldest_remote_unreliable_monotonic_timestamp,
+ .oldest_local_unreliable_monotonic_timestamp =
+ oldest_local_unreliable_monotonic_timestamp}});
+ } else {
+ boot_times_it->second.emplace_back(
BootPairTimes{.oldest_remote_monotonic_timestamp =
oldest_remote_monotonic_timestamp,
.oldest_local_monotonic_timestamp =
@@ -665,24 +686,6 @@
oldest_remote_unreliable_monotonic_timestamp,
.oldest_local_unreliable_monotonic_timestamp =
oldest_local_unreliable_monotonic_timestamp});
- } else {
- // If we found an existing entry, update the min to be the min of all
- // records. This lets us combine info from multiple part files.
- if (oldest_remote_monotonic_timestamp <
- boot_times_it->second.oldest_remote_monotonic_timestamp) {
- boot_times_it->second.oldest_remote_monotonic_timestamp =
- oldest_remote_monotonic_timestamp;
- boot_times_it->second.oldest_local_monotonic_timestamp =
- oldest_local_monotonic_timestamp;
- }
- if (oldest_remote_unreliable_monotonic_timestamp <
- boot_times_it->second
- .oldest_remote_unreliable_monotonic_timestamp) {
- boot_times_it->second.oldest_remote_unreliable_monotonic_timestamp =
- oldest_remote_unreliable_monotonic_timestamp;
- boot_times_it->second.oldest_local_unreliable_monotonic_timestamp =
- oldest_local_unreliable_monotonic_timestamp;
- }
}
}
}
@@ -800,38 +803,139 @@
// Now, we have a bunch of remote boots for the same local boot and
// remote node. We want to sort them by observed local time. This will
- // tell us which ones happened first.
- std::vector<std::pair<std::string, BootPairTimes>> source_boot_times;
- for (const auto &boot_time : source_nodes.second) {
- source_boot_times.emplace_back(boot_time);
+ // tell us which ones happened first. Hold on to the max time on that
+ // node too so we can check for overlapping boots.
+ std::vector<std::tuple<std::string, BootPairTimes, BootPairTimes>>
+ source_boot_times;
+ for (const auto &boot_time_list : source_nodes.second) {
+ // Track the first boot time we have evidence of.
+ BootPairTimes boot_time = boot_time_list.second[0];
+ // And the last one so we can look for overlapping boots.
+ BootPairTimes max_boot_time = boot_time_list.second[0];
+ for (size_t i = 0; i < boot_time_list.second.size(); ++i) {
+ const BootPairTimes &next_boot_time = boot_time_list.second[i];
+ if (next_boot_time.oldest_local_unreliable_monotonic_timestamp !=
+ aos::monotonic_clock::max_time) {
+ VLOG(1)
+ << "Remote time "
+ << next_boot_time.oldest_remote_unreliable_monotonic_timestamp
+ << " " << boot_time_list.first;
+ VLOG(1)
+ << "Local time "
+ << next_boot_time.oldest_local_unreliable_monotonic_timestamp
+ << " " << boot_time_list.first;
+ }
+ // If we found an existing entry, update the min to be the min of
+ // all records. This lets us combine info from multiple part files.
+ if (next_boot_time.oldest_remote_monotonic_timestamp <
+ boot_time.oldest_remote_monotonic_timestamp) {
+ boot_time.oldest_remote_monotonic_timestamp =
+ next_boot_time.oldest_remote_monotonic_timestamp;
+ boot_time.oldest_local_monotonic_timestamp =
+ next_boot_time.oldest_local_monotonic_timestamp;
+ }
+ if ((next_boot_time.oldest_remote_monotonic_timestamp >
+ max_boot_time.oldest_remote_monotonic_timestamp ||
+ max_boot_time.oldest_remote_monotonic_timestamp ==
+ aos::monotonic_clock::max_time) &&
+ next_boot_time.oldest_remote_monotonic_timestamp !=
+ aos::monotonic_clock::max_time) {
+ max_boot_time.oldest_remote_monotonic_timestamp =
+ next_boot_time.oldest_remote_monotonic_timestamp;
+ max_boot_time.oldest_local_monotonic_timestamp =
+ next_boot_time.oldest_local_monotonic_timestamp;
+ }
+ if (next_boot_time.oldest_remote_unreliable_monotonic_timestamp <
+ boot_time.oldest_remote_unreliable_monotonic_timestamp) {
+ boot_time.oldest_remote_unreliable_monotonic_timestamp =
+ next_boot_time.oldest_remote_unreliable_monotonic_timestamp;
+ boot_time.oldest_local_unreliable_monotonic_timestamp =
+ next_boot_time.oldest_local_unreliable_monotonic_timestamp;
+ }
+ if ((next_boot_time.oldest_remote_unreliable_monotonic_timestamp >
+ max_boot_time
+ .oldest_remote_unreliable_monotonic_timestamp ||
+ max_boot_time.oldest_remote_unreliable_monotonic_timestamp ==
+ aos::monotonic_clock::max_time) &&
+ next_boot_time.oldest_remote_unreliable_monotonic_timestamp !=
+ aos::monotonic_clock::max_time) {
+ max_boot_time.oldest_remote_unreliable_monotonic_timestamp =
+ next_boot_time.oldest_remote_unreliable_monotonic_timestamp;
+ max_boot_time.oldest_local_unreliable_monotonic_timestamp =
+ next_boot_time.oldest_local_unreliable_monotonic_timestamp;
+ }
+ }
+ source_boot_times.emplace_back(
+ std::make_tuple(boot_time_list.first, boot_time, max_boot_time));
}
std::sort(
source_boot_times.begin(), source_boot_times.end(),
- [](const std::pair<std::string, BootPairTimes> &a,
- const std::pair<std::string, BootPairTimes> &b) {
+ [](const std::tuple<std::string, BootPairTimes, BootPairTimes> &a,
+ const std::tuple<std::string, BootPairTimes, BootPairTimes> &b) {
// There are cases where we will only have a reliable timestamp.
// In that case, we need to use oldest_local_monotonic_timestamp.
// But, that may result in collisions if the same message gets
// forwarded to both boots, so it will have the same timestamp.
// Solve that by breaking the tie with the unreliable messages.
- if (a.second.oldest_local_monotonic_timestamp ==
- b.second.oldest_local_monotonic_timestamp) {
- CHECK_NE(a.second.oldest_local_unreliable_monotonic_timestamp,
- b.second.oldest_local_unreliable_monotonic_timestamp);
- return a.second.oldest_local_unreliable_monotonic_timestamp <
- b.second.oldest_local_unreliable_monotonic_timestamp;
+ if (std::get<1>(a).oldest_local_monotonic_timestamp ==
+ std::get<1>(b).oldest_local_monotonic_timestamp) {
+ CHECK_NE(
+ std::get<1>(a).oldest_local_unreliable_monotonic_timestamp,
+ std::get<1>(b).oldest_local_unreliable_monotonic_timestamp);
+ return std::get<1>(a)
+ .oldest_local_unreliable_monotonic_timestamp <
+ std::get<1>(b)
+ .oldest_local_unreliable_monotonic_timestamp;
} else {
- return a.second.oldest_local_monotonic_timestamp <
- b.second.oldest_local_monotonic_timestamp;
+ return std::get<1>(a).oldest_local_monotonic_timestamp <
+ std::get<1>(b).oldest_local_monotonic_timestamp;
}
});
+ // The last time from the source node on the logger node.
+ // This is used to track overlapping boots since this should always
+ // increase.
+ aos::monotonic_clock::time_point last_boot_time =
+ aos::monotonic_clock::min_time;
+
// Now take our sorted list and build up constraints so we can solve.
for (size_t boot_id = 0; boot_id < source_boot_times.size();
++boot_id) {
- const std::pair<std::string, BootPairTimes> &boot_time =
- source_boot_times[boot_id];
- const std::string &source_boot_uuid = boot_time.first;
+ const std::tuple<std::string, BootPairTimes, BootPairTimes>
+ &boot_time = source_boot_times[boot_id];
+ const std::string &source_boot_uuid = std::get<0>(boot_time);
+
+ // Enforce that the last time observed in the headers on the previous
+ // boot is less than the first time on the next boot. This equates to
+ // there being no overlap between the two boots.
+ if (std::get<1>(boot_time)
+ .oldest_local_unreliable_monotonic_timestamp <
+ last_boot_time) {
+ for (size_t fatal_boot_id = 0;
+ fatal_boot_id < source_boot_times.size(); ++fatal_boot_id) {
+ const std::tuple<std::string, BootPairTimes, BootPairTimes>
+ &fatal_boot_time = source_boot_times[fatal_boot_id];
+ const std::string &fatal_source_boot_uuid =
+ std::get<0>(fatal_boot_time);
+ LOG(ERROR) << "Boot " << fatal_boot_id << ", "
+ << fatal_source_boot_uuid << " on " << source_node_name
+ << " spans ["
+ << std::get<1>(fatal_boot_time)
+ .oldest_local_unreliable_monotonic_timestamp
+ << ", "
+ << std::get<2>(fatal_boot_time)
+ .oldest_local_unreliable_monotonic_timestamp
+ << "] on logger " << logger_node_name;
+ }
+ LOG(FATAL) << "Found overlapping boots on " << source_node_name
+ << " logged on " << logger_node_name << ", "
+ << std::get<1>(boot_time)
+ .oldest_local_unreliable_monotonic_timestamp
+ << " < " << last_boot_time;
+ }
+
+ last_boot_time = std::get<2>(boot_time)
+ .oldest_local_unreliable_monotonic_timestamp;
auto source_node_boot_constraints_it =
boot_constraints.find(source_node_name);
@@ -854,9 +958,9 @@
&per_node_boot_constraints =
source_node_boot_constraints_it->second.constraints;
- const std::pair<std::string, BootPairTimes> &prior_boot_time =
- source_boot_times[boot_id - 1];
- const std::string &prior_boot_uuid = prior_boot_time.first;
+ const std::tuple<std::string, BootPairTimes, BootPairTimes>
+ &prior_boot_time = source_boot_times[boot_id - 1];
+ const std::string &prior_boot_uuid = std::get<0>(prior_boot_time);
auto first_per_boot_constraints =
per_node_boot_constraints.find(prior_boot_uuid);
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 0293aa1..59b106e 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -336,15 +336,18 @@
return message_header_builder.Finish();
}
-SpanReader::SpanReader(std::string_view filename) : filename_(filename) {
+SpanReader::SpanReader(std::string_view filename, bool quiet)
+ : filename_(filename) {
decoder_ = std::make_unique<DummyDecoder>(filename);
static constexpr std::string_view kXz = ".xz";
static constexpr std::string_view kSnappy = SnappyDecoder::kExtension;
if (filename.substr(filename.size() - kXz.size()) == kXz) {
#if ENABLE_LZMA
- decoder_ = std::make_unique<ThreadedLzmaDecoder>(std::move(decoder_));
+ decoder_ =
+ std::make_unique<ThreadedLzmaDecoder>(std::move(decoder_), quiet);
#else
+ (void)quiet;
LOG(FATAL) << "Reading xz-compressed files not supported on this platform";
#endif
} else if (filename.substr(filename.size() - kSnappy.size()) == kSnappy) {
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 3a84726..f2bdc42 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -202,7 +202,7 @@
// Class to read chunks out of a log file.
class SpanReader {
public:
- SpanReader(std::string_view filename);
+ SpanReader(std::string_view filename, bool quiet = false);
std::string_view filename() const { return filename_; }
diff --git a/aos/events/logging/logfile_utils_test.cc b/aos/events/logging/logfile_utils_test.cc
index 9be70c6..c9ec37e 100644
--- a/aos/events/logging/logfile_utils_test.cc
+++ b/aos/events/logging/logfile_utils_test.cc
@@ -2506,6 +2506,220 @@
}
}
+class SortingDeathTest : public SortingElementTest {
+ public:
+ SortingDeathTest()
+ : SortingElementTest(),
+ part0_(MakeHeader(config_, R"({
+ /* 100ms */
+ "max_out_of_order_duration": 100000000,
+ "node": {
+ "name": "pi1"
+ },
+ "logger_node": {
+ "name": "pi1"
+ },
+ "monotonic_start_time": 1000000,
+ "realtime_start_time": 1000000000000,
+ "logger_monotonic_start_time": 1000000,
+ "logger_realtime_start_time": 1000000000000,
+ "log_event_uuid": "30ef1283-81d7-4004-8c36-1c162dbcb2b2",
+ "parts_uuid": "ee4f5a98-77d0-4e01-af2f-bbb29e098ede",
+ "parts_index": 0,
+ "logger_instance_uuid": "1c3142ad-10a5-408d-a760-b63b73d3b904",
+ "logger_node_boot_uuid": "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "source_node_boot_uuid": "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "boot_uuids": [
+ "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "6ba4f28d-21a2-4d7f-83f4-ee365cf86464",
+ ""
+ ],
+ "oldest_remote_monotonic_timestamps": [
+ 9223372036854775807,
+ 9223372036854775807,
+ 9223372036854775807
+ ],
+ "oldest_local_monotonic_timestamps": [
+ 9223372036854775807,
+ 9223372036854775807,
+ 9223372036854775807
+ ],
+ "oldest_remote_unreliable_monotonic_timestamps": [
+ 9223372036854775807,
+ 0,
+ 9223372036854775807
+ ],
+ "oldest_local_unreliable_monotonic_timestamps": [
+ 9223372036854775807,
+ 0,
+ 9223372036854775807
+ ]
+})")),
+ part1_(MakeHeader(config_, R"({
+ /* 100ms */
+ "max_out_of_order_duration": 100000000,
+ "node": {
+ "name": "pi1"
+ },
+ "logger_node": {
+ "name": "pi1"
+ },
+ "monotonic_start_time": 1000000,
+ "realtime_start_time": 1000000000000,
+ "logger_monotonic_start_time": 1000000,
+ "logger_realtime_start_time": 1000000000000,
+ "log_event_uuid": "30ef1283-81d7-4004-8c36-1c162dbcb2b2",
+ "parts_uuid": "ee4f5a98-77d0-4e01-af2f-bbb29e098ede",
+ "parts_index": 1,
+ "logger_instance_uuid": "1c3142ad-10a5-408d-a760-b63b73d3b904",
+ "logger_node_boot_uuid": "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "source_node_boot_uuid": "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "boot_uuids": [
+ "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "b728d27a-9181-4eac-bfc1-5d09b80469d2",
+ ""
+ ],
+ "oldest_remote_monotonic_timestamps": [
+ 9223372036854775807,
+ 9223372036854775807,
+ 9223372036854775807
+ ],
+ "oldest_local_monotonic_timestamps": [
+ 9223372036854775807,
+ 9223372036854775807,
+ 9223372036854775807
+ ],
+ "oldest_remote_unreliable_monotonic_timestamps": [
+ 9223372036854775807,
+ 100000,
+ 9223372036854775807
+ ],
+ "oldest_local_unreliable_monotonic_timestamps": [
+ 9223372036854775807,
+ 100000,
+ 9223372036854775807
+ ]
+})")),
+ part2_(MakeHeader(config_, R"({
+ /* 100ms */
+ "max_out_of_order_duration": 100000000,
+ "node": {
+ "name": "pi1"
+ },
+ "logger_node": {
+ "name": "pi1"
+ },
+ "monotonic_start_time": 1000000,
+ "realtime_start_time": 1000000000000,
+ "logger_monotonic_start_time": 1000000,
+ "logger_realtime_start_time": 1000000000000,
+ "log_event_uuid": "30ef1283-81d7-4004-8c36-1c162dbcb2b2",
+ "parts_uuid": "ee4f5a98-77d0-4e01-af2f-bbb29e098ede",
+ "parts_index": 2,
+ "logger_instance_uuid": "1c3142ad-10a5-408d-a760-b63b73d3b904",
+ "logger_node_boot_uuid": "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "source_node_boot_uuid": "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "boot_uuids": [
+ "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "6ba4f28d-21a2-4d7f-83f4-ee365cf86464",
+ ""
+ ],
+ "oldest_remote_monotonic_timestamps": [
+ 9223372036854775807,
+ 9223372036854775807,
+ 9223372036854775807
+ ],
+ "oldest_local_monotonic_timestamps": [
+ 9223372036854775807,
+ 9223372036854775807,
+ 9223372036854775807
+ ],
+ "oldest_remote_unreliable_monotonic_timestamps": [
+ 9223372036854775807,
+ 200000,
+ 9223372036854775807
+ ],
+ "oldest_local_unreliable_monotonic_timestamps": [
+ 9223372036854775807,
+ 200000,
+ 9223372036854775807
+ ]
+})")),
+ part3_(MakeHeader(config_, R"({
+ /* 100ms */
+ "max_out_of_order_duration": 100000000,
+ "node": {
+ "name": "pi1"
+ },
+ "logger_node": {
+ "name": "pi1"
+ },
+ "monotonic_start_time": 1000000,
+ "realtime_start_time": 1000000000000,
+ "logger_monotonic_start_time": 1000000,
+ "logger_realtime_start_time": 1000000000000,
+ "log_event_uuid": "30ef1283-81d7-4004-8c36-1c162dbcb2b2",
+ "parts_uuid": "ee4f5a98-77d0-4e01-af2f-bbb29e098ede",
+ "parts_index": 3,
+ "logger_instance_uuid": "1c3142ad-10a5-408d-a760-b63b73d3b904",
+ "logger_node_boot_uuid": "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "source_node_boot_uuid": "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "boot_uuids": [
+ "a570df8b-5cc2-4dbe-89bd-286f9ddd02b7",
+ "b728d27a-9181-4eac-bfc1-5d09b80469d2",
+ ""
+ ],
+ "oldest_remote_monotonic_timestamps": [
+ 9223372036854775807,
+ 9223372036854775807,
+ 9223372036854775807
+ ],
+ "oldest_local_monotonic_timestamps": [
+ 9223372036854775807,
+ 9223372036854775807,
+ 9223372036854775807
+ ],
+ "oldest_remote_unreliable_monotonic_timestamps": [
+ 9223372036854775807,
+ 300000,
+ 9223372036854775807
+ ],
+ "oldest_local_unreliable_monotonic_timestamps": [
+ 9223372036854775807,
+ 300000,
+ 9223372036854775807
+ ]
+})")) {}
+
+ protected:
+ const aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> part0_;
+ const aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> part1_;
+ const aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> part2_;
+ const aos::SizePrefixedFlatbufferDetachedBuffer<LogFileHeader> part3_;
+};
+
+// Tests that if 2 computers go back and forth trying to be the same node, we
+// die in sorting instead of failing to estimate time.
+TEST_F(SortingDeathTest, FightingNodes) {
+ {
+ DetachedBufferWriter writer0(logfile0_, std::make_unique<DummyEncoder>());
+ writer0.QueueSpan(part0_.span());
+ DetachedBufferWriter writer1(logfile1_, std::make_unique<DummyEncoder>());
+ writer1.QueueSpan(part1_.span());
+ DetachedBufferWriter writer2(logfile2_, std::make_unique<DummyEncoder>());
+ writer2.QueueSpan(part2_.span());
+ DetachedBufferWriter writer3(logfile3_, std::make_unique<DummyEncoder>());
+ writer3.QueueSpan(part3_.span());
+ }
+
+ EXPECT_DEATH(
+ {
+ const std::vector<LogFile> parts =
+ SortParts({logfile0_, logfile1_, logfile2_, logfile3_});
+ },
+ "Found overlapping boots on");
+}
+
} // namespace testing
} // namespace logger
} // namespace aos
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index a1e8cc9..7a569eb 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -2273,7 +2273,7 @@
}
// Test that renaming the base, renames the folder.
-TEST_F(MultinodeLoggerTest, LoggerRenameFolder) {
+TEST_P(MultinodeLoggerTest, LoggerRenameFolder) {
util::UnlinkRecursive(tmp_dir_ + "/renamefolder");
util::UnlinkRecursive(tmp_dir_ + "/new-good");
time_converter_.AddMonotonic(
@@ -2614,9 +2614,9 @@
}
constexpr std::string_view kCombinedConfigSha1(
- "644a3ab079c3ddfb1ef866483cfca9ec015c4142202169081138f72664ffd589");
+ "9e07da76098ad1b755a7c3143aca300d66b6abb88745f6c36e603ef1441f0ad5");
constexpr std::string_view kSplitConfigSha1(
- "20bb9f6ee89519c4bd49986f0afe497d61c71b29d846f2c51f51727c9ef37415");
+ "85ef8be228bf4eb36f4d64ba68183b2a9a616bfb9b057e430d61e33bd273df86");
INSTANTIATE_TEST_SUITE_P(
All, MultinodeLoggerTest,
diff --git a/aos/events/logging/lzma_encoder.cc b/aos/events/logging/lzma_encoder.cc
index e9915af..f32d7e2 100644
--- a/aos/events/logging/lzma_encoder.cc
+++ b/aos/events/logging/lzma_encoder.cc
@@ -149,9 +149,11 @@
total_bytes_ += last_avail_out - stream_.avail_out;
}
-LzmaDecoder::LzmaDecoder(std::unique_ptr<DataDecoder> underlying_decoder)
+LzmaDecoder::LzmaDecoder(std::unique_ptr<DataDecoder> underlying_decoder,
+ bool quiet)
: underlying_decoder_(std::move(underlying_decoder)),
- stream_(LZMA_STREAM_INIT) {
+ stream_(LZMA_STREAM_INIT),
+ quiet_(quiet) {
compressed_data_.resize(kBufSize);
lzma_ret status =
@@ -200,9 +202,13 @@
if (!LzmaCodeIsOk(status, filename())) {
finished_ = true;
if (status == LZMA_DATA_ERROR) {
- LOG(WARNING) << filename() << " is corrupted.";
+ if (!quiet_ || VLOG_IS_ON(1)) {
+ LOG(WARNING) << filename() << " is corrupted.";
+ }
} else if (status == LZMA_BUF_ERROR) {
- LOG(WARNING) << filename() << " is truncated or corrupted.";
+ if (!quiet_ || VLOG_IS_ON(1)) {
+ LOG(WARNING) << filename() << " is truncated or corrupted.";
+ }
} else {
LOG(FATAL) << "Unknown error " << status << " when reading "
<< filename();
@@ -214,8 +220,8 @@
}
ThreadedLzmaDecoder::ThreadedLzmaDecoder(
- std::unique_ptr<DataDecoder> underlying_decoder)
- : decoder_(std::move(underlying_decoder)), decode_thread_([this] {
+ std::unique_ptr<DataDecoder> underlying_decoder, bool quiet)
+ : decoder_(std::move(underlying_decoder), quiet), decode_thread_([this] {
std::unique_lock lock(decode_mutex_);
while (true) {
// Wake if the queue is too small or we are finished.
diff --git a/aos/events/logging/lzma_encoder.h b/aos/events/logging/lzma_encoder.h
index fc6fcb6..7b7010a 100644
--- a/aos/events/logging/lzma_encoder.h
+++ b/aos/events/logging/lzma_encoder.h
@@ -54,9 +54,10 @@
public:
static constexpr std::string_view kExtension = ".xz";
- explicit LzmaDecoder(std::unique_ptr<DataDecoder> underlying_decoder);
- explicit LzmaDecoder(std::string_view filename)
- : LzmaDecoder(std::make_unique<DummyDecoder>(filename)) {}
+ explicit LzmaDecoder(std::unique_ptr<DataDecoder> underlying_decoder,
+ bool quiet = false);
+ explicit LzmaDecoder(std::string_view filename, bool quiet = false)
+ : LzmaDecoder(std::make_unique<DummyDecoder>(filename), quiet) {}
LzmaDecoder(const LzmaDecoder &) = delete;
LzmaDecoder(LzmaDecoder &&other) = delete;
LzmaDecoder &operator=(const LzmaDecoder &) = delete;
@@ -84,6 +85,9 @@
// Flag that represents whether or not all the data from the file has been
// successfully decoded.
bool finished_ = false;
+ // Flag to signal how quiet to be when logging potential issues around
+ // truncation.
+ const bool quiet_ = false;
};
// Decompresses data with liblzma in a new thread, up to a maximum queue
@@ -91,9 +95,10 @@
// or block until more data is queued or the stream finishes.
class ThreadedLzmaDecoder : public DataDecoder {
public:
- explicit ThreadedLzmaDecoder(std::string_view filename)
- : ThreadedLzmaDecoder(std::make_unique<DummyDecoder>(filename)) {}
- explicit ThreadedLzmaDecoder(std::unique_ptr<DataDecoder> underlying_decoder);
+ explicit ThreadedLzmaDecoder(std::string_view filename, bool quiet = false)
+ : ThreadedLzmaDecoder(std::make_unique<DummyDecoder>(filename), quiet) {}
+ explicit ThreadedLzmaDecoder(std::unique_ptr<DataDecoder> underlying_decoder,
+ bool quiet = false);
ThreadedLzmaDecoder(const ThreadedLzmaDecoder &) = delete;
ThreadedLzmaDecoder &operator=(const ThreadedLzmaDecoder &) = delete;
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 083e246..2a276e7 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -112,7 +112,7 @@
// TODO(Brian): This should be in the anonymous namespace, but that annoys GCC
// for some reason...
-class SimulatedWatcher : public WatcherState {
+class SimulatedWatcher : public WatcherState, public EventScheduler::Event {
public:
SimulatedWatcher(
SimulatedEventLoop *simulated_event_loop, EventScheduler *scheduler,
@@ -123,11 +123,13 @@
bool has_run() const;
+ void Handle() noexcept override;
+
void Startup(EventLoop * /*event_loop*/) override {}
void Schedule(std::shared_ptr<SimulatedMessage> message);
- void HandleEvent();
+ void HandleEvent() noexcept;
void SetSimulatedChannel(SimulatedChannel *channel) {
simulated_channel_ = channel;
@@ -153,9 +155,9 @@
: channel_(channel),
channel_storage_duration_(channel_storage_duration),
next_queue_index_(ipc_lib::QueueIndex::Zero(number_buffers())) {
- available_buffer_indices_.reserve(number_buffers());
+ available_buffer_indices_.resize(number_buffers());
for (int i = 0; i < number_buffers(); ++i) {
- available_buffer_indices_.push_back(i);
+ available_buffer_indices_[i] = i;
}
}
@@ -424,7 +426,7 @@
// Updates the state inside RawFetcher to point to the data in msg_.
void SetMsg(std::shared_ptr<SimulatedMessage> msg) {
- msg_ = msg;
+ msg_ = std::move(msg);
context_ = msg_->context;
if (channel()->read_method() != ReadMethod::PIN) {
context_.buffer_index = -1;
@@ -442,7 +444,7 @@
// Internal method for Simulation to add a message to the buffer.
void Enqueue(std::shared_ptr<SimulatedMessage> buffer) {
- msgs_.emplace_back(buffer);
+ msgs_.emplace_back(std::move(buffer));
if (fell_behind_ ||
msgs_.size() > static_cast<size_t>(simulated_channel_->queue_size())) {
fell_behind_ = true;
@@ -463,7 +465,8 @@
bool fell_behind_ = false;
};
-class SimulatedTimerHandler : public TimerHandler {
+class SimulatedTimerHandler : public TimerHandler,
+ public EventScheduler::Event {
public:
explicit SimulatedTimerHandler(EventScheduler *scheduler,
SimulatedEventLoop *simulated_event_loop,
@@ -473,7 +476,9 @@
void Setup(monotonic_clock::time_point base,
monotonic_clock::duration repeat_offset) override;
- void HandleEvent();
+ void HandleEvent() noexcept;
+
+ void Handle() noexcept override;
void Disable() override;
@@ -487,7 +492,8 @@
monotonic_clock::duration repeat_offset_;
};
-class SimulatedPhasedLoopHandler : public PhasedLoopHandler {
+class SimulatedPhasedLoopHandler : public PhasedLoopHandler,
+ public EventScheduler::Event {
public:
SimulatedPhasedLoopHandler(EventScheduler *scheduler,
SimulatedEventLoop *simulated_event_loop,
@@ -496,10 +502,12 @@
const monotonic_clock::duration offset);
~SimulatedPhasedLoopHandler();
- void HandleEvent();
+ void HandleEvent() noexcept;
void Schedule(monotonic_clock::time_point sleep_time) override;
+ void Handle() noexcept override;
+
private:
SimulatedEventLoop *simulated_event_loop_;
EventHandler<SimulatedPhasedLoopHandler> event_;
@@ -837,10 +845,10 @@
DoSchedule(event_time);
}
- msgs_.emplace_back(message);
+ msgs_.emplace_back(std::move(message));
}
-void SimulatedWatcher::HandleEvent() {
+void SimulatedWatcher::HandleEvent() noexcept {
const monotonic_clock::time_point monotonic_now =
simulated_event_loop_->monotonic_now();
VLOG(1) << simulated_event_loop_->distributed_now() << " "
@@ -887,15 +895,17 @@
}
}
+void SimulatedWatcher::Handle() noexcept {
+ DCHECK(token_ != scheduler_->InvalidToken());
+ token_ = scheduler_->InvalidToken();
+ simulated_event_loop_->HandleEvent();
+}
+
void SimulatedWatcher::DoSchedule(monotonic_clock::time_point event_time) {
CHECK(token_ == scheduler_->InvalidToken())
<< ": May not schedule multiple times";
token_ = scheduler_->Schedule(
- event_time + simulated_event_loop_->send_delay(), [this]() {
- DCHECK(token_ != scheduler_->InvalidToken());
- token_ = scheduler_->InvalidToken();
- simulated_event_loop_->HandleEvent();
- });
+ event_time + simulated_event_loop_->send_delay(), this);
}
void SimulatedChannel::MakeRawWatcher(SimulatedWatcher *watcher) {
@@ -941,14 +951,14 @@
next_queue_index_ = next_queue_index_.Increment();
- latest_message_ = message;
+ latest_message_ = std::move(message);
for (SimulatedWatcher *watcher : watchers_) {
if (watcher->has_run()) {
- watcher->Schedule(message);
+ watcher->Schedule(latest_message_);
}
}
for (auto &fetcher : fetchers_) {
- fetcher->Enqueue(message);
+ fetcher->Enqueue(latest_message_);
}
return queue_index;
}
@@ -1085,16 +1095,18 @@
simulated_event_loop_->monotonic_now();
base_ = base;
repeat_offset_ = repeat_offset;
- token_ = scheduler_->Schedule(std::max(base, monotonic_now), [this]() {
- DCHECK(token_ != scheduler_->InvalidToken());
- token_ = scheduler_->InvalidToken();
- simulated_event_loop_->HandleEvent();
- });
+ token_ = scheduler_->Schedule(std::max(base, monotonic_now), this);
event_.set_event_time(base_);
simulated_event_loop_->AddEvent(&event_);
}
-void SimulatedTimerHandler::HandleEvent() {
+void SimulatedTimerHandler::Handle() noexcept {
+ DCHECK(token_ != scheduler_->InvalidToken());
+ token_ = scheduler_->InvalidToken();
+ simulated_event_loop_->HandleEvent();
+}
+
+void SimulatedTimerHandler::HandleEvent() noexcept {
const monotonic_clock::time_point monotonic_now =
simulated_event_loop_->monotonic_now();
VLOG(1) << simulated_event_loop_->distributed_now() << " "
@@ -1111,11 +1123,7 @@
if (repeat_offset_ != monotonic_clock::zero()) {
// Reschedule.
while (base_ <= monotonic_now) base_ += repeat_offset_;
- token_ = scheduler_->Schedule(base_, [this]() {
- DCHECK(token_ != scheduler_->InvalidToken());
- token_ = scheduler_->InvalidToken();
- simulated_event_loop_->HandleEvent();
- });
+ token_ = scheduler_->Schedule(base_, this);
event_.set_event_time(base_);
simulated_event_loop_->AddEvent(&event_);
}
@@ -1152,7 +1160,7 @@
simulated_event_loop_->RemoveEvent(&event_);
}
-void SimulatedPhasedLoopHandler::HandleEvent() {
+void SimulatedPhasedLoopHandler::HandleEvent() noexcept {
monotonic_clock::time_point monotonic_now =
simulated_event_loop_->monotonic_now();
VLOG(1) << monotonic_now << " Phased loop " << simulated_event_loop_->name()
@@ -1171,6 +1179,12 @@
}
}
+void SimulatedPhasedLoopHandler::Handle() noexcept {
+ DCHECK(token_ != scheduler_->InvalidToken());
+ token_ = scheduler_->InvalidToken();
+ simulated_event_loop_->HandleEvent();
+}
+
void SimulatedPhasedLoopHandler::Schedule(
monotonic_clock::time_point sleep_time) {
// The allocations in here are due to infrastructure and don't count in the no
@@ -1180,11 +1194,7 @@
scheduler_->Deschedule(token_);
token_ = scheduler_->InvalidToken();
}
- token_ = scheduler_->Schedule(sleep_time, [this]() {
- DCHECK(token_ != scheduler_->InvalidToken());
- token_ = scheduler_->InvalidToken();
- simulated_event_loop_->HandleEvent();
- });
+ token_ = scheduler_->Schedule(sleep_time, this);
event_.set_event_time(sleep_time);
simulated_event_loop_->AddEvent(&event_);
}
diff --git a/aos/events/simulated_event_loop_test.cc b/aos/events/simulated_event_loop_test.cc
index 696c16e..277e783 100644
--- a/aos/events/simulated_event_loop_test.cc
+++ b/aos/events/simulated_event_loop_test.cc
@@ -138,6 +138,16 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config;
};
+class FunctionEvent : public EventScheduler::Event {
+ public:
+ FunctionEvent(std::function<void()> fn) : fn_(fn) {}
+
+ void Handle() noexcept override { fn_(); }
+
+ private:
+ std::function<void()> fn_;
+};
+
// Test that creating an event and running the scheduler runs the event.
TEST(EventSchedulerTest, ScheduleEvent) {
int counter = 0;
@@ -145,12 +155,12 @@
EventScheduler scheduler(0);
scheduler_scheduler.AddEventScheduler(&scheduler);
- scheduler.Schedule(monotonic_clock::epoch() + chrono::seconds(1),
- [&counter]() { counter += 1; });
+ FunctionEvent e([&counter]() { counter += 1; });
+ scheduler.Schedule(monotonic_clock::epoch() + chrono::seconds(1), &e);
scheduler_scheduler.Run();
EXPECT_EQ(counter, 1);
- auto token = scheduler.Schedule(monotonic_clock::epoch() + chrono::seconds(2),
- [&counter]() { counter += 1; });
+ auto token =
+ scheduler.Schedule(monotonic_clock::epoch() + chrono::seconds(2), &e);
scheduler.Deschedule(token);
scheduler_scheduler.Run();
EXPECT_EQ(counter, 1);
@@ -163,8 +173,9 @@
EventScheduler scheduler(0);
scheduler_scheduler.AddEventScheduler(&scheduler);
- auto token = scheduler.Schedule(monotonic_clock::epoch() + chrono::seconds(1),
- [&counter]() { counter += 1; });
+ FunctionEvent e([&counter]() { counter += 1; });
+ auto token =
+ scheduler.Schedule(monotonic_clock::epoch() + chrono::seconds(1), &e);
scheduler.Deschedule(token);
scheduler_scheduler.Run();
EXPECT_EQ(counter, 0);
@@ -175,8 +186,7 @@
TestMessage::Builder test_message_builder =
builder.MakeBuilder<TestMessage>();
test_message_builder.add_value(value);
- ASSERT_EQ(builder.Send(test_message_builder.Finish()),
- RawSender::Error::kOk);
+ ASSERT_EQ(builder.Send(test_message_builder.Finish()), RawSender::Error::kOk);
}
// Test that sending a message after running gets properly notified.
@@ -508,6 +518,8 @@
for (const message_bridge::ServerConnection *connection :
*stats.connections()) {
EXPECT_EQ(connection->state(), message_bridge::State::CONNECTED);
+ EXPECT_EQ(connection->connection_count(), 1u);
+ EXPECT_EQ(connection->connected_since_time(), 0);
EXPECT_TRUE(connection->has_boot_uuid());
if (connection->node()->name()->string_view() == "pi2") {
EXPECT_GT(connection->sent_packets(), 50);
@@ -537,6 +549,8 @@
EXPECT_GT(connection->sent_packets(), 50);
EXPECT_TRUE(connection->has_monotonic_offset());
EXPECT_EQ(connection->monotonic_offset(), 0);
+ EXPECT_EQ(connection->connection_count(), 1u);
+ EXPECT_EQ(connection->connected_since_time(), 0);
++pi2_server_statistics_count;
});
@@ -554,6 +568,8 @@
EXPECT_GE(connection->sent_packets(), 5);
EXPECT_TRUE(connection->has_monotonic_offset());
EXPECT_EQ(connection->monotonic_offset(), 0);
+ EXPECT_EQ(connection->connection_count(), 1u);
+ EXPECT_EQ(connection->connected_since_time(), 0);
++pi3_server_statistics_count;
});
@@ -578,6 +594,8 @@
EXPECT_EQ(connection->partial_deliveries(), 0);
EXPECT_TRUE(connection->has_monotonic_offset());
EXPECT_EQ(connection->monotonic_offset(), 150000);
+ EXPECT_EQ(connection->connection_count(), 1u);
+ EXPECT_EQ(connection->connected_since_time(), 0);
}
++pi1_client_statistics_count;
});
@@ -596,6 +614,8 @@
EXPECT_EQ(connection->partial_deliveries(), 0);
EXPECT_TRUE(connection->has_monotonic_offset());
EXPECT_EQ(connection->monotonic_offset(), 150000);
+ EXPECT_EQ(connection->connection_count(), 1u);
+ EXPECT_EQ(connection->connected_since_time(), 0);
++pi2_client_statistics_count;
});
@@ -613,6 +633,8 @@
EXPECT_EQ(connection->partial_deliveries(), 0);
EXPECT_TRUE(connection->has_monotonic_offset());
EXPECT_EQ(connection->monotonic_offset(), 150000);
+ EXPECT_EQ(connection->connection_count(), 1u);
+ EXPECT_EQ(connection->connected_since_time(), 0);
++pi3_client_statistics_count;
});
@@ -1012,6 +1034,9 @@
if (connection->state() != message_bridge::State::CONNECTED) {
return false;
}
+ EXPECT_TRUE(connection->has_boot_uuid());
+ EXPECT_TRUE(connection->has_connected_since_time());
+ EXPECT_TRUE(connection->has_connection_count());
}
return true;
}
@@ -1024,15 +1049,42 @@
if (connection->state() == message_bridge::State::CONNECTED) {
return false;
}
+ EXPECT_FALSE(connection->has_boot_uuid());
+ EXPECT_FALSE(connection->has_connected_since_time());
} else {
if (connection->state() != message_bridge::State::CONNECTED) {
return false;
}
+ EXPECT_TRUE(connection->has_boot_uuid());
+ EXPECT_TRUE(connection->has_connected_since_time());
+ EXPECT_TRUE(connection->has_connection_count());
}
}
return true;
}
+int ConnectedCount(const message_bridge::ClientStatistics *client_statistics,
+ std::string_view target) {
+ for (const message_bridge::ClientConnection *connection :
+ *client_statistics->connections()) {
+ if (connection->node()->name()->string_view() == target) {
+ return connection->connection_count();
+ }
+ }
+ return 0;
+}
+
+int ConnectedCount(const message_bridge::ServerStatistics *server_statistics,
+ std::string_view target) {
+ for (const message_bridge::ServerConnection *connection :
+ *server_statistics->connections()) {
+ if (connection->node()->name()->string_view() == target) {
+ return connection->connection_count();
+ }
+ }
+ return 0;
+}
+
// Test that disconnecting nodes actually disconnects them.
TEST_P(RemoteMessageSimulatedEventLoopTest, MultinodeDisconnect) {
SimulatedEventLoopFactory simulated_event_loop_factory(&config.message());
@@ -1237,6 +1289,33 @@
simulated_event_loop_factory.RunFor(chrono::seconds(2));
+ EXPECT_TRUE(pi1_server_statistics_fetcher.Fetch());
+ EXPECT_TRUE(pi1_client_statistics_fetcher.Fetch());
+ EXPECT_TRUE(pi2_server_statistics_fetcher.Fetch());
+ EXPECT_TRUE(pi2_client_statistics_fetcher.Fetch());
+ EXPECT_TRUE(pi3_server_statistics_fetcher.Fetch());
+ EXPECT_TRUE(pi3_client_statistics_fetcher.Fetch());
+
+ EXPECT_EQ(ConnectedCount(pi1_server_statistics_fetcher.get(), "pi3"), 2u)
+ << " : " << aos::FlatbufferToJson(pi1_server_statistics_fetcher.get());
+ EXPECT_EQ(ConnectedCount(pi1_server_statistics_fetcher.get(), "pi2"), 1u)
+ << " : " << aos::FlatbufferToJson(pi1_server_statistics_fetcher.get());
+ EXPECT_EQ(ConnectedCount(pi1_client_statistics_fetcher.get(), "pi3"), 1u)
+ << " : " << aos::FlatbufferToJson(pi1_client_statistics_fetcher.get());
+ EXPECT_EQ(ConnectedCount(pi1_client_statistics_fetcher.get(), "pi2"), 1u)
+ << " : " << aos::FlatbufferToJson(pi1_client_statistics_fetcher.get());
+
+ EXPECT_EQ(ConnectedCount(pi2_server_statistics_fetcher.get(), "pi1"), 1u)
+ << " : " << aos::FlatbufferToJson(pi2_server_statistics_fetcher.get());
+ EXPECT_EQ(ConnectedCount(pi2_client_statistics_fetcher.get(), "pi1"), 1u)
+ << " : " << aos::FlatbufferToJson(pi2_client_statistics_fetcher.get());
+
+ EXPECT_EQ(ConnectedCount(pi3_server_statistics_fetcher.get(), "pi1"), 1u)
+ << " : " << aos::FlatbufferToJson(pi3_server_statistics_fetcher.get());
+ EXPECT_EQ(ConnectedCount(pi3_client_statistics_fetcher.get(), "pi1"), 2u)
+ << " : " << aos::FlatbufferToJson(pi3_client_statistics_fetcher.get());
+
+
EXPECT_EQ(pi1_pong_counter.count(), 601u);
EXPECT_EQ(pi2_pong_counter.count(), 601u);
@@ -1260,22 +1339,16 @@
EXPECT_EQ(CountAll(remote_timestamps_pi2_on_pi1), 661);
EXPECT_EQ(CountAll(remote_timestamps_pi1_on_pi2), 661);
- EXPECT_TRUE(pi1_server_statistics_fetcher.Fetch());
EXPECT_TRUE(AllConnected(pi1_server_statistics_fetcher.get()))
<< " : " << aos::FlatbufferToJson(pi1_server_statistics_fetcher.get());
- EXPECT_TRUE(pi1_client_statistics_fetcher.Fetch());
EXPECT_TRUE(AllConnected(pi1_client_statistics_fetcher.get()))
<< " : " << aos::FlatbufferToJson(pi1_client_statistics_fetcher.get());
- EXPECT_TRUE(pi2_server_statistics_fetcher.Fetch());
EXPECT_TRUE(AllConnected(pi2_server_statistics_fetcher.get()))
<< " : " << aos::FlatbufferToJson(pi2_server_statistics_fetcher.get());
- EXPECT_TRUE(pi2_client_statistics_fetcher.Fetch());
EXPECT_TRUE(AllConnected(pi2_client_statistics_fetcher.get()))
<< " : " << aos::FlatbufferToJson(pi2_client_statistics_fetcher.get());
- EXPECT_TRUE(pi3_server_statistics_fetcher.Fetch());
EXPECT_TRUE(AllConnected(pi3_server_statistics_fetcher.get()))
<< " : " << aos::FlatbufferToJson(pi3_server_statistics_fetcher.get());
- EXPECT_TRUE(pi3_client_statistics_fetcher.Fetch());
EXPECT_TRUE(AllConnected(pi3_client_statistics_fetcher.get()))
<< " : " << aos::FlatbufferToJson(pi3_client_statistics_fetcher.get());
}
@@ -1553,10 +1626,13 @@
int pi1_server_statistics_count = 0;
bool first_pi1_server_statistics = true;
+ int boot_number = 0;
+ monotonic_clock::time_point expected_connection_time = pi1->monotonic_now();
pi1_remote_timestamp->MakeWatcher(
- "/pi1/aos", [&pi1_server_statistics_count, &expected_boot_uuid,
- &first_pi1_server_statistics](
- const message_bridge::ServerStatistics &stats) {
+ "/pi1/aos",
+ [&pi1_server_statistics_count, &expected_boot_uuid,
+ &expected_connection_time, &first_pi1_server_statistics,
+ &boot_number](const message_bridge::ServerStatistics &stats) {
VLOG(1) << "pi1 ServerStatistics " << FlatbufferToJson(&stats);
for (const message_bridge::ServerConnection *connection :
*stats.connections()) {
@@ -1572,6 +1648,10 @@
EXPECT_EQ(expected_boot_uuid,
UUID::FromString(connection->boot_uuid()))
<< " : Got " << aos::FlatbufferToJson(&stats);
+ EXPECT_EQ(monotonic_clock::time_point(chrono::nanoseconds(
+ connection->connected_since_time())),
+ expected_connection_time);
+ EXPECT_EQ(boot_number + 1, connection->connection_count());
++pi1_server_statistics_count;
}
}
@@ -1580,7 +1660,8 @@
int pi1_client_statistics_count = 0;
pi1_remote_timestamp->MakeWatcher(
- "/pi1/aos", [&pi1_client_statistics_count](
+ "/pi1/aos", [&pi1_client_statistics_count, &expected_boot_uuid,
+ &expected_connection_time, &boot_number](
const message_bridge::ClientStatistics &stats) {
VLOG(1) << "pi1 ClientStatistics " << FlatbufferToJson(&stats);
for (const message_bridge::ClientConnection *connection :
@@ -1588,18 +1669,33 @@
EXPECT_EQ(connection->state(), message_bridge::State::CONNECTED);
if (connection->node()->name()->string_view() == "pi2") {
++pi1_client_statistics_count;
+ EXPECT_EQ(expected_boot_uuid,
+ UUID::FromString(connection->boot_uuid()))
+ << " : Got " << aos::FlatbufferToJson(&stats);
+ EXPECT_EQ(monotonic_clock::time_point(chrono::nanoseconds(
+ connection->connected_since_time())),
+ expected_connection_time);
+ EXPECT_EQ(boot_number + 1, connection->connection_count());
+ } else {
+ EXPECT_EQ(connection->connected_since_time(), 0);
+ EXPECT_EQ(1, connection->connection_count());
}
}
});
// Confirm that reboot changes the UUID.
- pi2->OnShutdown([&expected_boot_uuid, pi2, pi2_boot1]() {
- expected_boot_uuid = pi2_boot1;
- LOG(INFO) << "OnShutdown triggered for pi2";
- pi2->OnStartup([&expected_boot_uuid, pi2]() {
- EXPECT_EQ(expected_boot_uuid, pi2->boot_uuid());
- });
- });
+ pi2->OnShutdown(
+ [&expected_boot_uuid, &boot_number, &expected_connection_time, pi1, pi2,
+ pi2_boot1]() {
+ expected_boot_uuid = pi2_boot1;
+ ++boot_number;
+ LOG(INFO) << "OnShutdown triggered for pi2";
+ pi2->OnStartup(
+ [&expected_boot_uuid, &expected_connection_time, pi1, pi2]() {
+ EXPECT_EQ(expected_boot_uuid, pi2->boot_uuid());
+ expected_connection_time = pi1->monotonic_now();
+ });
+ });
// Let a couple of ServerStatistics messages show up before rebooting.
factory.RunFor(chrono::milliseconds(2002));
diff --git a/aos/events/simulated_network_bridge.cc b/aos/events/simulated_network_bridge.cc
index 96a614d..4c9208e 100644
--- a/aos/events/simulated_network_bridge.cc
+++ b/aos/events/simulated_network_bridge.cc
@@ -257,9 +257,9 @@
fetcher_->context().queue_index, fetcher_->context().source_boot_uuid));
// And simulate message_bridge's offset recovery.
- client_status_->SampleFilter(client_index_,
- fetcher_->context().monotonic_event_time,
- sender_->monotonic_sent_time());
+ client_status_->SampleFilter(
+ client_index_, fetcher_->context().monotonic_event_time,
+ sender_->monotonic_sent_time(), fetcher_->context().source_boot_uuid);
client_connection_->mutate_received_packets(
client_connection_->received_packets() + 1);
@@ -631,9 +631,16 @@
for (ServerConnection *connection : server_status->server_connection()) {
if (connection) {
if (boot_uuids_[node_index] != UUID::Zero()) {
- connection->mutate_state(server_state_[node_index]);
+ switch (server_state_[node_index]) {
+ case message_bridge::State::DISCONNECTED:
+ server_status->Disconnect(node_index);
+ break;
+ case message_bridge::State::CONNECTED:
+ server_status->Connect(node_index, event_loop->monotonic_now());
+ break;
+ }
} else {
- connection->mutate_state(message_bridge::State::DISCONNECTED);
+ server_status->Disconnect(node_index);
}
}
++node_index;
@@ -666,9 +673,18 @@
const size_t client_node_index = configuration::GetNodeIndex(
node_factory_->configuration(), client_node);
if (boot_uuids_[client_node_index] != UUID::Zero()) {
- client_connection->mutate_state(client_state_[client_node_index]);
+ if (client_connection->state() != client_state_[client_node_index]) {
+ switch (client_state_[client_node_index]) {
+ case message_bridge::State::DISCONNECTED:
+ client_status->Disconnect(i);
+ break;
+ case message_bridge::State::CONNECTED:
+ client_status->Connect(i);
+ break;
+ }
+ }
} else {
- client_connection->mutate_state(message_bridge::State::DISCONNECTED);
+ client_status->Disconnect(i);
}
}
diff --git a/aos/events/simulated_network_bridge.h b/aos/events/simulated_network_bridge.h
index 231bba1..3a7a4e8 100644
--- a/aos/events/simulated_network_bridge.h
+++ b/aos/events/simulated_network_bridge.h
@@ -86,7 +86,6 @@
SetEventLoop(node_factory_->MakeEventLoop("message_bridge"));
}
- void ClearEventLoop() { SetEventLoop(nullptr); }
void SetEventLoop(std::unique_ptr<aos::EventLoop> loop);
void SetSendData(std::function<void(const Context &)> fn) {
@@ -109,19 +108,38 @@
ServerConnection *connection =
server_status->FindServerConnection(node);
if (connection) {
- connection->mutate_state(server_state_[node_index]);
- server_status->ResetFilter(node_index);
- server_status->SetBootUUID(node_index, boot_uuid);
+ if (boot_uuid == UUID::Zero()) {
+ server_status->Disconnect(node_index);
+ server_status->ResetFilter(node_index);
+ } else {
+ switch (server_state_[node_index]) {
+ case message_bridge::State::DISCONNECTED:
+ server_status->Disconnect(node_index);
+ break;
+ case message_bridge::State::CONNECTED:
+ server_status->Connect(node_index, event_loop->monotonic_now());
+ break;
+ }
+ server_status->ResetFilter(node_index);
+ server_status->SetBootUUID(node_index, boot_uuid);
+ }
}
}
if (client_status) {
const int client_index =
client_status->FindClientIndex(node->name()->string_view());
- ClientConnection *client_connection =
- client_status->GetClientConnection(client_index);
- if (client_connection) {
- client_status->SampleReset(client_index);
- client_connection->mutate_state(client_state_[node_index]);
+ client_status->SampleReset(client_index);
+ if (boot_uuid == UUID::Zero()) {
+ client_status->Disconnect(client_index);
+ } else {
+ switch (client_state_[node_index]) {
+ case message_bridge::State::CONNECTED:
+ client_status->Connect(client_index);
+ break;
+ case message_bridge::State::DISCONNECTED:
+ client_status->Disconnect(client_index);
+ break;
+ }
}
}
}
@@ -135,7 +153,17 @@
server_status->FindServerConnection(destination);
if (connection == nullptr) return;
- connection->mutate_state(state);
+ if (state == connection->state()) {
+ return;
+ }
+ switch (state) {
+ case message_bridge::State::DISCONNECTED:
+ server_status->Disconnect(node_index);
+ break;
+ case message_bridge::State::CONNECTED:
+ server_status->Connect(node_index, event_loop->monotonic_now());
+ break;
+ }
}
}
@@ -144,12 +172,23 @@
configuration::GetNodeIndex(node_factory_->configuration(), source);
client_state_[node_index] = state;
if (client_status) {
+ const int client_index =
+ client_status->FindClientIndex(source->name()->string_view());
ClientConnection *connection =
client_status->GetClientConnection(source);
- if (connection == nullptr) return;
-
- connection->mutate_state(state);
+ // TODO(austin): Are there cases where we want to dedup 2 CONNECTED
+ // calls?
+ if (connection->state() != state) {
+ switch (state) {
+ case message_bridge::State::CONNECTED:
+ client_status->Connect(client_index);
+ break;
+ case message_bridge::State::DISCONNECTED:
+ client_status->Disconnect(client_index);
+ break;
+ }
+ }
}
}
diff --git a/aos/flatbuffers.h b/aos/flatbuffers.h
index 68a8ad5..cdf5339 100644
--- a/aos/flatbuffers.h
+++ b/aos/flatbuffers.h
@@ -7,6 +7,7 @@
#include "absl/types/span.h"
#include "aos/containers/resizeable_buffer.h"
#include "aos/macros.h"
+#include "aos/util/file.h"
#include "flatbuffers/flatbuffers.h" // IWYU pragma: export
#include "glog/logging.h"
@@ -501,6 +502,31 @@
span.size());
}
+// MMap a flatbuffer on disk.
+template <typename T>
+class FlatbufferMMap : public NonSizePrefixedFlatbuffer<T> {
+ public:
+ // Builds a Flatbuffer by mmaping the data from a flatbuffer saved on disk.
+ FlatbufferMMap(const std::string &flatbuffer_path,
+ util::FileOptions options = util::FileOptions::kReadable) {
+ span_ = util::MMapFile(flatbuffer_path, options);
+ }
+
+ // Copies the reference to the mapped memory.
+ FlatbufferMMap(const FlatbufferMMap &) = default;
+ FlatbufferMMap &operator=(const FlatbufferMMap<T> &other) = default;
+
+ // Moves the reference to the mapped memory from one pointer to another.
+ FlatbufferMMap(FlatbufferMMap &&) = default;
+ FlatbufferMMap &operator=(FlatbufferMMap<T> &&other) = default;
+
+ absl::Span<uint8_t> span() override { return *span_; }
+ absl::Span<const uint8_t> span() const override { return *span_; }
+
+ private:
+ std::shared_ptr<absl::Span<uint8_t>> span_;
+};
+
} // namespace aos
#endif // AOS_FLATBUFFERS_H_
diff --git a/aos/flatbuffers_test.cc b/aos/flatbuffers_test.cc
index e3030f1..ae8d6e6 100644
--- a/aos/flatbuffers_test.cc
+++ b/aos/flatbuffers_test.cc
@@ -1,9 +1,10 @@
#include "aos/flatbuffers.h"
-#include "gtest/gtest.h"
-
+#include "absl/strings/str_cat.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/json_to_flatbuffer_generated.h"
+#include "aos/testing/tmpdir.h"
+#include "gtest/gtest.h"
namespace aos {
namespace testing {
@@ -21,5 +22,59 @@
EXPECT_FALSE(empty.Verify());
}
+// Tests the ability to map a flatbuffer on disk to memory
+TEST(FlatbufferMMapTest, Verify) {
+ FlatbufferDetachedBuffer<Configuration> fb =
+ JsonToFlatbuffer<Configuration>("{\"foo_int\": 3}");
+
+ const std::string fb_path = absl::StrCat(TestTmpDir(), "/fb.bfbs");
+ WriteFlatbufferToFile(fb_path, fb);
+
+ FlatbufferMMap<Configuration> fb_mmap(fb_path);
+ EXPECT_TRUE(fb.Verify());
+ EXPECT_TRUE(fb_mmap.Verify());
+ ASSERT_EQ(fb_mmap.message().foo_int(), 3);
+
+ // Verify that copying works
+ {
+ FlatbufferMMap<Configuration> fb_mmap2(fb_path);
+ fb_mmap2 = fb_mmap;
+ EXPECT_TRUE(fb_mmap.Verify());
+ EXPECT_TRUE(fb_mmap2.Verify());
+ ASSERT_EQ(fb_mmap2.message().foo_int(), 3);
+ ASSERT_EQ(fb_mmap.message().foo_int(), 3);
+ }
+ EXPECT_TRUE(fb_mmap.Verify());
+ ASSERT_EQ(fb_mmap.message().foo_int(), 3);
+
+ // Verify that moving works
+ {
+ FlatbufferMMap<Configuration> fb_mmap3(fb_path);
+ fb_mmap3 = std::move(fb_mmap);
+ EXPECT_TRUE(fb_mmap3.Verify());
+ ASSERT_EQ(fb_mmap3.message().foo_int(), 3);
+ }
+}
+
+// Tests the ability to modify a flatbuffer mmaped from on disk in memory
+TEST(FlatbufferMMapTest, Writeable) {
+ FlatbufferDetachedBuffer<Configuration> fb =
+ JsonToFlatbuffer<Configuration>("{\"foo_int\": 3}");
+
+ const std::string fb_path = absl::StrCat(TestTmpDir(), "/fb.bfbs");
+ WriteFlatbufferToFile(fb_path, fb);
+
+ {
+ FlatbufferMMap<Configuration> fb_mmap(fb_path,
+ util::FileOptions::kWriteable);
+ fb_mmap.mutable_message()->mutate_foo_int(5);
+ }
+
+ {
+ FlatbufferMMap<Configuration> fb_mmap(fb_path);
+ EXPECT_EQ(fb_mmap.message().foo_int(), 5);
+ }
+}
+
} // namespace testing
} // namespace aos
diff --git a/aos/ipc_lib/BUILD b/aos/ipc_lib/BUILD
index c24fcee..c124fe0 100644
--- a/aos/ipc_lib/BUILD
+++ b/aos/ipc_lib/BUILD
@@ -12,6 +12,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
+ ":shm_observers",
"//aos:macros",
"//aos:thread_local",
"//aos/util:compiler_memory_barrier",
@@ -146,6 +147,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
+ ":shm_observers",
"@com_github_google_glog//:glog",
],
)
@@ -225,6 +227,7 @@
":event",
":lockless_queue",
":queue_racer",
+ ":shm_observers",
":signalfd",
"//aos/events:epoll",
"//aos/libc:aos_strsignal",
@@ -359,3 +362,13 @@
"//aos/time",
],
)
+
+cc_library(
+ name = "shm_observers",
+ srcs = [
+ "shm_observers.cc",
+ ],
+ hdrs = [
+ "shm_observers.h",
+ ],
+)
diff --git a/aos/ipc_lib/aos_sync.cc b/aos/ipc_lib/aos_sync.cc
index 39ea4c6..861a6fe 100644
--- a/aos/ipc_lib/aos_sync.cc
+++ b/aos/ipc_lib/aos_sync.cc
@@ -20,6 +20,8 @@
#include <cstdint>
#include <cstring>
+#include "aos/ipc_lib/shm_observers.h"
+
#ifdef AOS_SANITIZER_thread
#include <sanitizer/tsan_interface_atomic.h>
#endif
@@ -33,7 +35,7 @@
#include "aos/util/compiler_memory_barrier.h"
#include "glog/logging.h"
-using ::aos::linux_code::ipc_lib::FutexAccessorObserver;
+using ::aos::linux_code::ipc_lib::RunShmObservers;
// This code was originally based on
// <https://www.akkadia.org/drepper/futex.pdf>, but is has since evolved a lot.
@@ -115,17 +117,24 @@
// result or a negated errno value. -1..-4095 mean errors and not successful
// results, which is guaranteed by the kernel.
//
-// They each have optimized versions for ARM EABI (the syscall interface is
-// different for non-EABI ARM, so that is the right thing to test for) that
-// don't go through syscall(2) or errno.
-// These use register variables to get the values in the right registers to
-// actually make the syscall.
+// They each have optimized versions for some architectures which don't go
+// through syscall(2) or errno. These use register variables to get the values
+// in the right registers to actually make the syscall.
-// The actual macro that we key off of to use the inline versions or not.
+// The actual macros that we key off of to use the inline versions or not.
#if defined(__ARM_EABI__)
+// The syscall interface is different for non-EABI ARM, so we test specifically
+// for EABI.
#define ARM_EABI_INLINE_SYSCALL 1
+#define AARCH64_INLINE_SYSCALL 0
+#elif defined(__aarch64__)
+// Linux only has one supported syscall ABI on aarch64, which is the one we
+// support.
+#define ARM_EABI_INLINE_SYSCALL 0
+#define AARCH64_INLINE_SYSCALL 1
#else
#define ARM_EABI_INLINE_SYSCALL 0
+#define AARCH64_INLINE_SYSCALL 0
#endif
// Used for FUTEX_WAIT, FUTEX_LOCK_PI, and FUTEX_TRYLOCK_PI.
@@ -144,6 +153,19 @@
"r"(timeout_reg), "r"(syscall_number)
: "memory");
return result;
+#elif AARCH64_INLINE_SYSCALL
+ register aos_futex *addr1_reg __asm__("x0") = addr1;
+ register int op_reg __asm__("x1") = op;
+ register int val1_reg __asm__("x2") = val1;
+ register const struct timespec *timeout_reg __asm__("x3") = timeout;
+ register int syscall_number __asm__("x8") = SYS_futex;
+ register int result __asm__("x0");
+ __asm__ volatile("svc #0"
+ : "=r"(result)
+ : "r"(addr1_reg), "r"(op_reg), "r"(val1_reg),
+ "r"(timeout_reg), "r"(syscall_number)
+ : "memory");
+ return result;
#else
const int r = syscall(SYS_futex, addr1, op, val1, timeout);
if (r == -1) return -errno;
@@ -164,6 +186,18 @@
"r"(syscall_number)
: "memory");
return result;
+#elif AARCH64_INLINE_SYSCALL
+ register aos_futex *addr1_reg __asm__("x0") = addr1;
+ register int op_reg __asm__("x1") = FUTEX_WAKE;
+ register int val1_reg __asm__("x2") = val1;
+ register int syscall_number __asm__("x8") = SYS_futex;
+ register int result __asm__("x0");
+ __asm__ volatile("svc #0"
+ : "=r"(result)
+ : "r"(addr1_reg), "r"(op_reg), "r"(val1_reg),
+ "r"(syscall_number)
+ : "memory");
+ return result;
#else
const int r = syscall(SYS_futex, addr1, FUTEX_WAKE, val1);
if (r == -1) return -errno;
@@ -190,6 +224,22 @@
"r"(syscall_number)
: "memory");
return result;
+#elif AARCH64_INLINE_SYSCALL
+ register aos_futex *addr1_reg __asm__("x0") = addr1;
+ register int op_reg __asm__("x1") = FUTEX_CMP_REQUEUE_PI;
+ register int num_wake_reg __asm__("x2") = num_wake;
+ register int num_requeue_reg __asm__("x3") = num_requeue;
+ register aos_futex *m_reg __asm__("x4") = m;
+ register uint32_t val_reg __asm__("x5") = val;
+ register int syscall_number __asm__("x8") = SYS_futex;
+ register int result __asm__("x0");
+ __asm__ volatile("svc #0"
+ : "=r"(result)
+ : "r"(addr1_reg), "r"(op_reg), "r"(num_wake_reg),
+ "r"(num_requeue_reg), "r"(m_reg), "r"(val_reg),
+ "r"(syscall_number)
+ : "memory");
+ return result;
#else
const int r = syscall(SYS_futex, addr1, FUTEX_CMP_REQUEUE_PI, num_wake,
num_requeue, m, val);
@@ -215,6 +265,20 @@
"r"(timeout_reg), "r"(m_reg), "r"(syscall_number)
: "memory");
return result;
+#elif AARCH64_INLINE_SYSCALL
+ register aos_condition *addr1_reg __asm__("x0") = addr1;
+ register int op_reg __asm__("x1") = FUTEX_WAIT_REQUEUE_PI;
+ register uint32_t start_val_reg __asm__("x2") = start_val;
+ register const struct timespec *timeout_reg __asm__("x3") = timeout;
+ register aos_futex *m_reg __asm__("x4") = m;
+ register int syscall_number __asm__("x8") = SYS_futex;
+ register int result __asm__("x0");
+ __asm__ volatile("svc #0"
+ : "=r"(result)
+ : "r"(addr1_reg), "r"(op_reg), "r"(start_val_reg),
+ "r"(timeout_reg), "r"(m_reg), "r"(syscall_number)
+ : "memory");
+ return result;
#else
const int r =
syscall(SYS_futex, addr1, FUTEX_WAIT_REQUEUE_PI, start_val, timeout, m);
@@ -234,6 +298,16 @@
: "r"(addr1_reg), "r"(op_reg), "r"(syscall_number)
: "memory");
return result;
+#elif AARCH64_INLINE_SYSCALL
+ register aos_futex *addr1_reg __asm__("x0") = addr1;
+ register int op_reg __asm__("x1") = FUTEX_UNLOCK_PI;
+ register int syscall_number __asm__("x8") = SYS_futex;
+ register int result __asm__("x0");
+ __asm__ volatile("svc #0"
+ : "=r"(result)
+ : "r"(addr1_reg), "r"(op_reg), "r"(syscall_number)
+ : "memory");
+ return result;
#else
const int r = syscall(SYS_futex, addr1, FUTEX_UNLOCK_PI);
if (r == -1) return -errno;
@@ -608,34 +682,6 @@
my_robust_list::Init();
}
-FutexAccessorObserver before_observer = nullptr, after_observer = nullptr;
-
-// RAII class which runs before_observer during construction and after_observer
-// during destruction.
-class RunObservers {
- public:
- template <class T>
- RunObservers(T *address, bool write)
- : address_(static_cast<void *>(
- const_cast<typename ::std::remove_cv<T>::type *>(address))),
- write_(write) {
- if (__builtin_expect(before_observer != nullptr, false)) {
- before_observer(address_, write_);
- }
- }
- ~RunObservers() {
- if (__builtin_expect(after_observer != nullptr, false)) {
- after_observer(address_, write_);
- }
- }
-
- private:
- void *const address_;
- const bool write_;
-
- DISALLOW_COPY_AND_ASSIGN(RunObservers);
-};
-
// Finishes the locking of a mutex by potentially clearing FUTEX_OWNER_DIED in
// the futex and returning the correct value.
inline int mutex_finish_lock(aos_mutex *m) {
@@ -654,7 +700,7 @@
// own my_robust_list::Adder.
inline int mutex_do_get(aos_mutex *m, bool signals_fail,
const struct timespec *timeout, uint32_t tid) {
- RunObservers run_observers(m, true);
+ RunShmObservers run_observers(m, true);
if (kPrintOperations) {
printf("%" PRId32 ": %p do_get\n", tid, m);
}
@@ -722,7 +768,7 @@
// number_requeue is the number of waiters to requeue (probably INT_MAX or 0). 1
// will always be woken.
void condition_wake(aos_condition *c, aos_mutex *m, int number_requeue) {
- RunObservers run_observers(c, true);
+ RunShmObservers run_observers(c, true);
// Make it so that anybody just going to sleep won't.
// This is where we might accidentally wake more than just 1 waiter with 1
// signal():
@@ -766,7 +812,7 @@
int mutex_grab(aos_mutex *m) { return mutex_get(m, false, NULL); }
void mutex_unlock(aos_mutex *m) {
- RunObservers run_observers(m, true);
+ RunShmObservers run_observers(m, true);
const uint32_t tid = get_tid();
if (kPrintOperations) {
printf("%" PRId32 ": %p unlock\n", tid, m);
@@ -802,7 +848,7 @@
}
int mutex_trylock(aos_mutex *m) {
- RunObservers run_observers(m, true);
+ RunShmObservers run_observers(m, true);
const uint32_t tid = get_tid();
if (kPrintOperations) {
printf("%" PRId32 ": %p trylock\n", tid, m);
@@ -857,14 +903,14 @@
}
my_robust_list::Adder adder(m);
{
- RunObservers run_observers(m, true);
+ RunShmObservers run_observers(m, true);
CHECK(compare_and_swap(&m->futex, 0, tid));
}
adder.Add();
}
void death_notification_release(aos_mutex *m) {
- RunObservers run_observers(m, true);
+ RunShmObservers run_observers(m, true);
#ifndef NDEBUG
// Verify it's "locked", like it should be.
@@ -889,7 +935,7 @@
}
int condition_wait(aos_condition *c, aos_mutex *m, struct timespec *end_time) {
- RunObservers run_observers(c, false);
+ RunShmObservers run_observers(c, false);
const uint32_t tid = get_tid();
const uint32_t wait_start = __atomic_load_n(c, __ATOMIC_SEQ_CST);
@@ -969,7 +1015,7 @@
}
int futex_wait_timeout(aos_futex *m, const struct timespec *timeout) {
- RunObservers run_observers(m, false);
+ RunShmObservers run_observers(m, false);
const int ret = sys_futex_wait(FUTEX_WAIT, m, 0, timeout);
if (ret != 0) {
if (ret == -EINTR) {
@@ -988,7 +1034,7 @@
int futex_wait(aos_futex *m) { return futex_wait_timeout(m, NULL); }
int futex_set_value(aos_futex *m, uint32_t value) {
- RunObservers run_observers(m, false);
+ RunShmObservers run_observers(m, false);
ANNOTATE_HAPPENS_BEFORE(m);
__atomic_store_n(m, value, __ATOMIC_SEQ_CST);
const int r = sys_futex_wake(m, INT_MAX - 4096);
@@ -1012,15 +1058,6 @@
namespace linux_code {
namespace ipc_lib {
-// Sets functions to run befor eand after all futex operations.
-// This is important when doing robustness testing because the memory has to be
-// made writable for the whole futex operation, otherwise it never succeeds.
-void SetFutexAccessorObservers(FutexAccessorObserver before,
- FutexAccessorObserver after) {
- before_observer = before;
- after_observer = after;
-}
-
// Sets an extra offset between mutexes and the value we use for them in the
// robust list (only the forward pointers). This is used to work around a kernel
// bug by keeping a second set of mutexes which is always writable so the kernel
diff --git a/aos/ipc_lib/aos_sync.h b/aos/ipc_lib/aos_sync.h
index a61fcc1..c5fede1 100644
--- a/aos/ipc_lib/aos_sync.h
+++ b/aos/ipc_lib/aos_sync.h
@@ -182,12 +182,6 @@
namespace linux_code {
namespace ipc_lib {
-typedef void (*FutexAccessorObserver)(void *address, bool write);
-
-// Set functions which get called before and after all futex operations.
-void SetFutexAccessorObservers(FutexAccessorObserver before,
- FutexAccessorObserver after);
-
// Set the offset to use for putting addresses into the robust list.
// This is necessary to work around a kernel bug where it hangs when trying to
// deal with a futex on the robust list when its memory has been changed to
diff --git a/aos/ipc_lib/index.h b/aos/ipc_lib/index.h
index a47121e..c6a485f 100644
--- a/aos/ipc_lib/index.h
+++ b/aos/ipc_lib/index.h
@@ -2,9 +2,11 @@
#define AOS_IPC_LIB_INDEX_H_
#include <sys/types.h>
+
#include <atomic>
#include <string>
+#include "aos/ipc_lib/shm_observers.h"
#include "glog/logging.h"
namespace aos {
@@ -155,6 +157,7 @@
// Swaps expected for index atomically. Returns true on success, false
// otherwise.
inline bool CompareAndExchangeStrong(QueueIndex expected, QueueIndex index) {
+ linux_code::ipc_lib::RunShmObservers run_observers(&index_, true);
return index_.compare_exchange_strong(expected.index_, index.index_,
::std::memory_order_acq_rel);
}
@@ -242,6 +245,7 @@
// Swaps expected for index atomically. Returns true on success, false
// otherwise.
bool CompareAndExchangeStrong(Index expected, Index index) {
+ linux_code::ipc_lib::RunShmObservers run_observers(&index_, true);
return index_.compare_exchange_strong(expected.index_, index.index_,
::std::memory_order_acq_rel);
}
diff --git a/aos/ipc_lib/lockless_queue_death_test.cc b/aos/ipc_lib/lockless_queue_death_test.cc
index 3ed2ce5..2217811 100644
--- a/aos/ipc_lib/lockless_queue_death_test.cc
+++ b/aos/ipc_lib/lockless_queue_death_test.cc
@@ -1,7 +1,11 @@
#include <dlfcn.h>
+#include <elf.h>
#include <linux/futex.h>
#include <sys/mman.h>
+#include <sys/procfs.h>
+#include <sys/ptrace.h>
#include <sys/syscall.h>
+#include <sys/uio.h>
#include <unistd.h>
#include <wait.h>
@@ -14,6 +18,7 @@
#include "aos/ipc_lib/aos_sync.h"
#include "aos/ipc_lib/lockless_queue.h"
#include "aos/ipc_lib/lockless_queue_memory.h"
+#include "aos/ipc_lib/shm_observers.h"
#include "aos/libc/aos_strsignal.h"
#include "aos/realtime.h"
#include "aos/testing/prevent_exit.h"
@@ -124,14 +129,6 @@
};
::std::atomic<GlobalState *> global_state;
-#ifndef __ARM_EABI__
-#ifndef __x86_64__
-#error This code only works on amd64.
-#endif
-
-// The "trap bit" which enables single-stepping for x86.
-const greg_t kTrapFlag = 1 << 8;
-
// Returns true if the address is in the queue memory chunk.
bool IsInLocklessQueueMemory(void *address) {
GlobalState *my_global_state = global_state.load(::std::memory_order_relaxed);
@@ -199,7 +196,6 @@
const int saved_errno = errno;
SIMPLE_ASSERT(signal == SIGSEGV, "wrong signal for SIGSEGV handler");
- ucontext_t *const context = static_cast<ucontext_t *>(context_void);
// Only process memory addresses in our shared memory block.
if (!IsInLocklessQueueMemory(siginfo->si_addr)) {
if (CallChainedAction(old_segv_handler, signal, siginfo, context_void)) {
@@ -215,9 +211,16 @@
HandleWrite(siginfo->si_addr);
ShmProtectOrDie(PROT_READ | PROT_WRITE);
- context->uc_mcontext.gregs[REG_EFL] |= kTrapFlag;
my_global_state->state = DieAtState::kWriting;
errno = saved_errno;
+
+#if defined(__x86_64__)
+ __asm__ __volatile__("int $3" ::: "memory", "cc");
+#elif defined(__aarch64__)
+ __asm__ __volatile__("brk #0" ::: "memory", "cc");
+#else
+#error Unhandled architecture
+#endif
}
// A mutex lock is about to happen. Mark the memory rw, and check to see if we
@@ -234,14 +237,12 @@
// The SEGV handler has set a breakpoint 1 instruction in the future. This
// clears it, marks memory readonly, and continues.
-void trap_handler(int signal, siginfo_t *, void *context_void) {
+void trap_handler(int signal, siginfo_t *, void * /*context*/) {
GlobalState *my_global_state = global_state.load(::std::memory_order_relaxed);
const int saved_errno = errno;
SIMPLE_ASSERT(signal == SIGTRAP, "wrong signal for SIGTRAP handler");
- ucontext_t *const context = static_cast<ucontext_t *>(context_void);
-
- context->uc_mcontext.gregs[REG_EFL] &= ~kTrapFlag;
+ my_global_state->state = DieAtState::kWriting;
SIMPLE_ASSERT(my_global_state->state == DieAtState::kWriting,
"bad state for SIGTRAP");
ShmProtectOrDie(PROT_READ);
@@ -267,7 +268,9 @@
struct sigaction action;
memset(&action, 0, sizeof(action));
action.sa_sigaction = handler;
- action.sa_flags = SA_RESTART | SA_SIGINFO;
+ // We don't do a full normal signal handler exit with ptrace, so SA_NODEFER is
+ // necessary to keep our signal handler active.
+ action.sa_flags = SA_RESTART | SA_SIGINFO | SA_NODEFER;
#ifdef AOS_SANITIZER_thread
// Tsan messes with signal handlers to check for race conditions, and it
// causes problems, so we have to work around it for SIGTRAP.
@@ -286,8 +289,6 @@
PCHECK(sigaction(signal, &action, old_action) == 0);
}
-#endif // ifndef __ARM_EABI__
-
// gtest only allows creating fatal failures in functions returning void...
// status is from wait(2).
void DetectFatalFailures(int status) {
@@ -298,7 +299,7 @@
FAIL() << " child exited because of signal "
<< aos_strsignal(WTERMSIG(status));
} else {
- FAIL() << "child exited with status " << ::std::hex << status;
+ FAIL() << " child exited with status " << ::std::hex << status;
}
}
@@ -332,8 +333,9 @@
InstallHandler(SIGSEGV, segv_handler, &old_segv_handler);
InstallHandler(SIGTRAP, trap_handler, &old_trap_handler);
CHECK_EQ(old_trap_handler.sa_handler, SIG_DFL);
- linux_code::ipc_lib::SetFutexAccessorObservers(futex_before, futex_after);
+ linux_code::ipc_lib::SetShmAccessorObservers(futex_before, futex_after);
+ PCHECK(ptrace(PTRACE_TRACEME, 0, 0, 0) == 0);
ShmProtectOrDie(PROT_READ);
my_global_state->state = DieAtState::kRunning;
@@ -342,19 +344,84 @@
ShmProtectOrDie(PROT_READ | PROT_WRITE);
_exit(0);
} else {
+ // Annoying wrapper type because elf_gregset_t is an array, which C++
+ // handles poorly.
+ struct RestoreState {
+ RestoreState(elf_gregset_t regs_in) {
+ memcpy(regs, regs_in, sizeof(regs));
+ }
+ elf_gregset_t regs;
+ };
+ std::optional<RestoreState> restore_regs;
+ bool pass_trap = false;
// Wait until the child process dies.
while (true) {
int status;
pid_t waited_on = waitpid(pid, &status, 0);
if (waited_on == -1) {
if (errno == EINTR) continue;
- PCHECK(false) << ": waitpid(" << static_cast<intmax_t>(pid) << ", "
- << &status << ", 0) failed";
+ PCHECK(false) << ": waitpid(" << pid << ", " << &status
+ << ", 0) failed";
}
- if (waited_on != pid) {
- PCHECK(false) << ": waitpid got child "
- << static_cast<intmax_t>(waited_on) << " instead of "
- << static_cast<intmax_t>(pid);
+ CHECK_EQ(waited_on, pid)
+ << ": waitpid got child " << waited_on << " instead of " << pid;
+ if (WIFSTOPPED(status)) {
+ // The child was stopped via ptrace.
+ const int stop_signal = WSTOPSIG(status);
+ elf_gregset_t regs;
+ {
+ struct iovec iov;
+ iov.iov_base = ®s;
+ iov.iov_len = sizeof(regs);
+ PCHECK(ptrace(PTRACE_GETREGSET, pid, NT_PRSTATUS, &iov) == 0);
+ CHECK_EQ(iov.iov_len, sizeof(regs))
+ << ": ptrace regset is the wrong size";
+ }
+ if (stop_signal == SIGSEGV) {
+ // It's a SEGV, hopefully due to writing to the shared memory which is
+ // marked read-only. We record the instruction that faulted so we can
+ // look for it while single-stepping, then deliver the signal so the
+ // child can mark it read-write and then poke us to single-step that
+ // instruction.
+
+ CHECK(!restore_regs)
+ << ": Traced child got a SEGV while single-stepping";
+ // Save all the registers to resume execution at the current location
+ // in the child.
+ restore_regs = RestoreState(regs);
+ PCHECK(ptrace(PTRACE_CONT, pid, nullptr, SIGSEGV) == 0);
+ continue;
+ }
+ if (stop_signal == SIGTRAP) {
+ if (pass_trap) {
+ // This is the new SIGTRAP we generated, which we just want to pass
+ // through so the child's signal handler can restore the memory to
+ // read-only
+ PCHECK(ptrace(PTRACE_CONT, pid, nullptr, SIGTRAP) == 0);
+ pass_trap = false;
+ continue;
+ }
+ if (restore_regs) {
+ // Restore the state we saved before delivering the SEGV, and then
+ // single-step that one instruction.
+ struct iovec iov;
+ iov.iov_base = &restore_regs->regs;
+ iov.iov_len = sizeof(restore_regs->regs);
+ PCHECK(ptrace(PTRACE_SETREGSET, pid, NT_PRSTATUS, &iov) == 0);
+ restore_regs = std::nullopt;
+ PCHECK(ptrace(PTRACE_SINGLESTEP, pid, nullptr, nullptr) == 0);
+ continue;
+ }
+ // We executed the single instruction that originally faulted, so
+ // now deliver a SIGTRAP to the child so it can mark the memory
+ // read-only again.
+ pass_trap = true;
+ PCHECK(kill(pid, SIGTRAP) == 0);
+ PCHECK(ptrace(PTRACE_CONT, pid, nullptr, nullptr) == 0);
+ continue;
+ }
+ LOG(FATAL) << "Traced child was stopped with unexpected signal: "
+ << static_cast<int>(WSTOPSIG(status));
}
if (WIFEXITED(status)) {
if (WEXITSTATUS(status) == 0) return true;
@@ -465,6 +532,7 @@
if (RunFunctionDieAtAndCheck(config, prepare, function, check, &test_failed,
die_at, prepare_in_child, expected_writes,
nullptr)) {
+ LOG(INFO) << "Tested " << die_at << " death points";
return;
}
if (test_failed) {
@@ -566,7 +634,7 @@
}
if (print) {
- printf("Bad version:\n");
+ LOG(INFO) << "Bad version:";
PrintLocklessQueueMemory(memory);
}
@@ -574,7 +642,7 @@
LocklessQueueSender::Make(queue).value();
if (print) {
- printf("Cleaned up version:\n");
+ LOG(INFO) << "Cleaned up version:";
PrintLocklessQueueMemory(memory);
}
diff --git a/aos/ipc_lib/shm_observers.cc b/aos/ipc_lib/shm_observers.cc
new file mode 100644
index 0000000..04a6214
--- /dev/null
+++ b/aos/ipc_lib/shm_observers.cc
@@ -0,0 +1,17 @@
+#include "aos/ipc_lib/shm_observers.h"
+
+namespace aos {
+namespace linux_code {
+namespace ipc_lib {
+
+ShmAccessorObserver before_observer = nullptr, after_observer = nullptr;
+
+void SetShmAccessorObservers(ShmAccessorObserver before,
+ ShmAccessorObserver after) {
+ before_observer = before;
+ after_observer = after;
+}
+
+} // namespace ipc_lib
+} // namespace linux_code
+} // namespace aos
diff --git a/aos/ipc_lib/shm_observers.h b/aos/ipc_lib/shm_observers.h
new file mode 100644
index 0000000..5780699
--- /dev/null
+++ b/aos/ipc_lib/shm_observers.h
@@ -0,0 +1,52 @@
+#ifndef AOS_IPC_LIB_SHM_OBSERVERS_H_
+#define AOS_IPC_LIB_SHM_OBSERVERS_H_
+
+#include <type_traits>
+
+namespace aos {
+namespace linux_code {
+namespace ipc_lib {
+
+typedef void (*ShmAccessorObserver)(void *address, bool write);
+
+extern ShmAccessorObserver before_observer, after_observer;
+
+// Sets functions to run before and after SHM write operations which may
+// involved multiple instructions. This is important when doing robustness
+// testing because the memory has to be made writable for the whole operation,
+// otherwise it never succeeds.
+void SetShmAccessorObservers(ShmAccessorObserver before,
+ ShmAccessorObserver after);
+
+// RAII class which runs before_observer during construction and after_observer
+// during destruction.
+class RunShmObservers {
+ public:
+ template <class T>
+ RunShmObservers(T *address, bool write)
+ : address_(static_cast<void *>(
+ const_cast<typename ::std::remove_cv<T>::type *>(address))),
+ write_(write) {
+ if (__builtin_expect(before_observer != nullptr, false)) {
+ before_observer(address_, write_);
+ }
+ }
+ ~RunShmObservers() {
+ if (__builtin_expect(after_observer != nullptr, false)) {
+ after_observer(address_, write_);
+ }
+ }
+
+ RunShmObservers(const RunShmObservers &) = delete;
+ RunShmObservers &operator=(const RunShmObservers &) = delete;
+
+ private:
+ void *const address_;
+ const bool write_;
+};
+
+} // namespace ipc_lib
+} // namespace linux_code
+} // namespace aos
+
+#endif // AOS_IPC_LIB_SHM_OBSERVERS_H_
diff --git a/aos/network/message_bridge_client.fbs b/aos/network/message_bridge_client.fbs
index 6efe3a9..40c0698 100644
--- a/aos/network/message_bridge_client.fbs
+++ b/aos/network/message_bridge_client.fbs
@@ -25,7 +25,16 @@
// (indicates congestion)
partial_deliveries:uint (id: 5);
- // TODO(austin): Per channel counts?
+ // Boot UUID of the server.
+ boot_uuid:string (id: 6);
+
+ // Time at which we connected to the server as nanoseconds on the local
+ // monotonic clock. This is not populated when not connected, and defaults
+ // to monotonic_clock::min_time.
+ connected_since_time:int64 = -9223372036854775808 (id: 7);
+
+ // Number of times we've established a connection to the server.
+ connection_count:uint (id: 8);
}
// Statistics for all clients.
diff --git a/aos/network/message_bridge_client_lib.cc b/aos/network/message_bridge_client_lib.cc
index f535c4f..9001cce 100644
--- a/aos/network/message_bridge_client_lib.cc
+++ b/aos/network/message_bridge_client_lib.cc
@@ -208,16 +208,14 @@
// the priority scheduler. This only needs to be done once per stream.
client_.SetPriorityScheduler(assoc_id);
- connection_->mutate_state(State::CONNECTED);
- client_status_->SampleReset(client_index_);
+ client_status_->Connect(client_index_);
}
void SctpClientConnection::NodeDisconnected() {
connect_timer_->Setup(
event_loop_->monotonic_now() + chrono::milliseconds(100),
chrono::milliseconds(100));
- connection_->mutate_state(State::DISCONNECTED);
- connection_->mutate_monotonic_offset(0);
+ client_status_->Disconnect(client_index_);
client_status_->SampleReset(client_index_);
}
@@ -270,7 +268,8 @@
client_index_,
monotonic_clock::time_point(
chrono::nanoseconds(remote_data->monotonic_sent_time())),
- sender->monotonic_sent_time());
+ sender->monotonic_sent_time(),
+ UUID::FromVector(remote_data->boot_uuid()));
if (stream_reply_with_timestamp_[stream]) {
// TODO(austin): Send back less if we are only acking. Maybe only a
diff --git a/aos/network/message_bridge_client_status.cc b/aos/network/message_bridge_client_status.cc
index 357026a..aa2484d 100644
--- a/aos/network/message_bridge_client_status.cc
+++ b/aos/network/message_bridge_client_status.cc
@@ -34,6 +34,9 @@
connection_builder.add_duplicate_packets(0);
connection_builder.add_monotonic_offset(0);
connection_builder.add_partial_deliveries(0);
+ connection_builder.add_connected_since_time(
+ monotonic_clock::min_time.time_since_epoch().count());
+ connection_builder.add_connection_count(0);
connection_offsets.emplace_back(connection_builder.Finish());
}
flatbuffers::Offset<
@@ -58,6 +61,7 @@
client_connection_offsets_.reserve(
statistics_.message().connections()->size());
filters_.resize(statistics_.message().connections()->size());
+ uuids_.resize(statistics_.message().connections()->size(), UUID::Zero());
statistics_timer_ = event_loop_->AddTimer([this]() { SendStatistics(); });
statistics_timer_->set_name("statistics");
@@ -69,6 +73,26 @@
});
}
+void MessageBridgeClientStatus::Disconnect(int client_index) {
+ ClientConnection *connection = GetClientConnection(client_index);
+
+ connection->mutate_state(State::DISCONNECTED);
+ connection->mutate_connected_since_time(
+ monotonic_clock::min_time.time_since_epoch().count());
+ connection->mutate_monotonic_offset(0);
+
+ uuids_[client_index] = UUID::Zero();
+}
+
+void MessageBridgeClientStatus::Connect(int client_index) {
+ ClientConnection *connection = GetClientConnection(client_index);
+
+ connection->mutate_state(State::CONNECTED);
+ connection->mutate_connected_since_time(
+ event_loop_->monotonic_now().time_since_epoch().count());
+ connection->mutate_connection_count(connection->connection_count() + 1);
+}
+
void MessageBridgeClientStatus::SendStatistics() {
if (!send_) {
return;
@@ -78,14 +102,23 @@
aos::Sender<ClientStatistics>::Builder builder = sender_.MakeBuilder();
client_connection_offsets_.clear();
- for (const ClientConnection *connection :
- *statistics_.message().connections()) {
+ for (size_t client_index = 0;
+ client_index < statistics_.message().connections()->size();
+ ++client_index) {
+ const ClientConnection *connection =
+ statistics_.message().connections()->Get(client_index);
flatbuffers::Offset<flatbuffers::String> node_name_offset =
builder.fbb()->CreateString(connection->node()->name()->string_view());
Node::Builder node_builder = builder.MakeBuilder<Node>();
node_builder.add_name(node_name_offset);
flatbuffers::Offset<Node> node_offset = node_builder.Finish();
+ flatbuffers::Offset<flatbuffers::String> uuid_offset = 0;
+
+ if (uuids_[client_index] != UUID::Zero()) {
+ uuid_offset = uuids_[client_index].PackString(builder.fbb());
+ }
+
ClientConnection::Builder client_connection_builder =
builder.MakeBuilder<ClientConnection>();
@@ -97,9 +130,24 @@
client_connection_builder.add_duplicate_packets(
connection->duplicate_packets());
}
+
+ if (connection->connected_since_time() !=
+ monotonic_clock::min_time.time_since_epoch().count()) {
+ client_connection_builder.add_connected_since_time(
+ connection->connected_since_time());
+ }
+
+ if (connection->connection_count() != 0) {
+ client_connection_builder.add_connection_count(
+ connection->connection_count());
+ }
client_connection_builder.add_partial_deliveries(
connection->partial_deliveries());
+ if (!uuid_offset.IsNull()) {
+ client_connection_builder.add_boot_uuid(uuid_offset);
+ }
+
// Strip out the monotonic offset if it isn't populated.
TimestampFilter *filter = &filters_[client_connection_offsets_.size()];
if (filter->has_sample()) {
@@ -152,7 +200,8 @@
void MessageBridgeClientStatus::SampleFilter(
int client_index,
const aos::monotonic_clock::time_point monotonic_sent_time,
- const aos::monotonic_clock::time_point monotonic_delivered_time) {
+ const aos::monotonic_clock::time_point monotonic_delivered_time,
+ const UUID &uuid) {
TimestampFilter *filter = &filters_[client_index];
const std::chrono::nanoseconds offset =
@@ -164,6 +213,8 @@
filter->set_base_offset(offset);
}
+ uuids_[client_index] = uuid;
+
// We can now measure the latency!
filter->Sample(monotonic_delivered_time, offset);
}
diff --git a/aos/network/message_bridge_client_status.h b/aos/network/message_bridge_client_status.h
index d38244a..9c21169 100644
--- a/aos/network/message_bridge_client_status.h
+++ b/aos/network/message_bridge_client_status.h
@@ -42,11 +42,17 @@
void SampleFilter(
int client_index,
const aos::monotonic_clock::time_point monotonic_sent_time,
- const aos::monotonic_clock::time_point monotonic_delivered_time);
+ const aos::monotonic_clock::time_point monotonic_delivered_time,
+ const UUID &uuid);
// Clears out the filter state.
void SampleReset(int client_index) { filters_[client_index].Reset(); }
+ // Disconnects the client.
+ void Disconnect(int client_index);
+ // Connects the client.
+ void Connect(int client_index);
+
// Disables sending out any statistics messages.
void DisableStatistics();
// Enables sending out any statistics messages.
@@ -73,6 +79,8 @@
std::vector<TimestampFilter> filters_;
+ std::vector<UUID> uuids_;
+
// If true, send out the messages.
bool send_ = true;
};
diff --git a/aos/network/message_bridge_server.fbs b/aos/network/message_bridge_server.fbs
index 128f1f3..031f801 100644
--- a/aos/network/message_bridge_server.fbs
+++ b/aos/network/message_bridge_server.fbs
@@ -34,7 +34,13 @@
// (indicates congestion)
partial_deliveries:uint (id: 6);
- // TODO(austin): Per channel counts?
+ // Time at which we connected to the client as nanoseconds on the local
+ // monotonic clock. This is not populated when not connected, and defaults
+ // to monotonic_clock::min_time.
+ connected_since_time:int64 = -9223372036854775808 (id: 7);
+
+ // Number of times we've established a connection to the server.
+ connection_count:uint (id: 8);
}
// Statistics for all connections to all the clients.
diff --git a/aos/network/message_bridge_server_lib.cc b/aos/network/message_bridge_server_lib.cc
index 41fb915..2ca8e48 100644
--- a/aos/network/message_bridge_server_lib.cc
+++ b/aos/network/message_bridge_server_lib.cc
@@ -171,10 +171,11 @@
void ChannelState::AddPeer(const Connection *connection, int node_index,
ServerConnection *server_connection_statistics,
+ MessageBridgeServerStatus *server_status,
bool logged_remotely,
aos::Sender<RemoteMessage> *timestamp_logger) {
peers_.emplace_back(connection, node_index, server_connection_statistics,
- logged_remotely, timestamp_logger);
+ server_status, logged_remotely, timestamp_logger);
}
int ChannelState::NodeDisconnected(sctp_assoc_t assoc_id) {
@@ -183,7 +184,6 @@
if (peer.sac_assoc_id == assoc_id) {
// TODO(austin): This will not handle multiple clients from
// a single node. But that should be rare.
- peer.server_connection_statistics->mutate_state(State::DISCONNECTED);
peer.sac_assoc_id = 0;
peer.stream = 0;
return peer.node_index;
@@ -192,21 +192,27 @@
return -1;
}
-int ChannelState::NodeConnected(const Node *node, sctp_assoc_t assoc_id,
- int stream, SctpServer *server) {
- VLOG(1) << "Connected to assoc_id: " << assoc_id;
+int ChannelState::NodeConnected(
+ const Node *node, sctp_assoc_t assoc_id, int stream, SctpServer *server,
+ aos::monotonic_clock::time_point monotonic_now) {
+ VLOG(1) << "Connected to assoc_id: " << assoc_id << " for stream " << stream;
for (ChannelState::Peer &peer : peers_) {
if (peer.connection->name()->string_view() == node->name()->string_view()) {
// There's a peer already connected. Disconnect them and take over.
if (peer.sac_assoc_id != 0) {
- LOG(WARNING) << "Peer " << peer.sac_assoc_id
- << " already connected, aborting old connection.";
- server->Abort(peer.sac_assoc_id);
+ if (peer.sac_assoc_id == assoc_id) {
+ LOG(WARNING) << "Reconnecting with the same ID, something got lost";
+ } else {
+ LOG(WARNING) << "Peer " << peer.sac_assoc_id
+ << " already connected, aborting old connection.";
+ server->Abort(peer.sac_assoc_id);
+ }
}
peer.sac_assoc_id = assoc_id;
peer.stream = stream;
- peer.server_connection_statistics->mutate_state(State::CONNECTED);
+ peer.server_status->Connect(peer.node_index, monotonic_now);
+
server->SetStreamPriority(assoc_id, stream, peer.connection->priority());
if (last_message_fetcher_ && peer.connection->time_to_live() == 0) {
last_message_fetcher_->Fetch();
@@ -323,6 +329,7 @@
connection->name()->string_view()),
server_status_.FindServerConnection(
connection->name()->string_view()),
+ &server_status_,
configuration::ChannelMessageIsLoggedOnNode(channel, other_node),
delivery_time_is_logged
? timestamp_loggers_.SenderForChannel(channel, connection)
@@ -392,6 +399,7 @@
->Get(node_index)
->name()
->string_view();
+ server_status_.Disconnect(node_index);
server_status_.ResetFilter(node_index);
server_status_.ClearBootUUID(node_index);
server_status_.ResetPartialDeliveries(node_index);
@@ -452,6 +460,7 @@
CHECK_LE(connect->channels_to_transfer()->size(),
static_cast<size_t>(max_channels()))
<< ": Client has more channels than we do";
+ monotonic_clock::time_point monotonic_now = event_loop_->monotonic_now();
// Account for the control channel and delivery times channel.
size_t channel_index = kControlStreams();
@@ -465,7 +474,7 @@
if (channel_state->Matches(channel)) {
node_index = channel_state->NodeConnected(
connect->node(), message->header.rcvinfo.rcv_assoc_id,
- channel_index, &server_);
+ channel_index, &server_, monotonic_now);
CHECK_NE(node_index, -1);
matched = true;
diff --git a/aos/network/message_bridge_server_lib.h b/aos/network/message_bridge_server_lib.h
index 6f96c02..e70d34a 100644
--- a/aos/network/message_bridge_server_lib.h
+++ b/aos/network/message_bridge_server_lib.h
@@ -38,11 +38,12 @@
struct Peer {
Peer(const Connection *new_connection, int new_node_index,
ServerConnection *new_server_connection_statistics,
- bool new_logged_remotely,
+ MessageBridgeServerStatus *new_server_status, bool new_logged_remotely,
aos::Sender<RemoteMessage> *new_timestamp_logger)
: connection(new_connection),
node_index(new_node_index),
server_connection_statistics(new_server_connection_statistics),
+ server_status(new_server_status),
timestamp_logger(new_timestamp_logger),
logged_remotely(new_logged_remotely) {}
@@ -53,6 +54,7 @@
const aos::Connection *connection;
const int node_index;
ServerConnection *server_connection_statistics;
+ MessageBridgeServerStatus *server_status;
aos::Sender<RemoteMessage> *timestamp_logger = nullptr;
// If true, this message will be logged on a receiving node. We need to
@@ -64,12 +66,13 @@
// Returns the node index which [dis]connected, or -1 if it didn't match.
int NodeDisconnected(sctp_assoc_t assoc_id);
int NodeConnected(const Node *node, sctp_assoc_t assoc_id, int stream,
- SctpServer *server);
+ SctpServer *server,
+ aos::monotonic_clock::time_point monotonic_now);
// Adds a new peer.
void AddPeer(const Connection *connection, int node_index,
ServerConnection *server_connection_statistics,
- bool logged_remotely,
+ MessageBridgeServerStatus *server_status, bool logged_remotely,
aos::Sender<RemoteMessage> *timestamp_logger);
// Returns true if this channel has the same name and type as the other
diff --git a/aos/network/message_bridge_server_status.cc b/aos/network/message_bridge_server_status.cc
index 09355b2..c82158b 100644
--- a/aos/network/message_bridge_server_status.cc
+++ b/aos/network/message_bridge_server_status.cc
@@ -37,6 +37,9 @@
connection_builder.add_sent_packets(0);
connection_builder.add_monotonic_offset(0);
connection_builder.add_partial_deliveries(0);
+ connection_builder.add_connected_since_time(
+ monotonic_clock::min_time.time_since_epoch().count());
+ connection_builder.add_connection_count(0);
connection_offsets.emplace_back(connection_builder.Finish());
}
flatbuffers::Offset<
@@ -162,6 +165,26 @@
server_connection_[node_index]->mutate_monotonic_offset(0);
}
+void MessageBridgeServerStatus::Connect(
+ int node_index, monotonic_clock::time_point monotonic_now) {
+ server_connection_[node_index]->mutate_state(State::CONNECTED);
+ // Only count connections if the timestamp changes. This deduplicates
+ // multiple channel connections at the same point in time.
+ if (server_connection_[node_index]->connected_since_time() !=
+ monotonic_now.time_since_epoch().count()) {
+ server_connection_[node_index]->mutate_connection_count(
+ server_connection_[node_index]->connection_count() + 1);
+ server_connection_[node_index]->mutate_connected_since_time(
+ monotonic_now.time_since_epoch().count());
+ }
+}
+
+void MessageBridgeServerStatus::Disconnect(int node_index) {
+ server_connection_[node_index]->mutate_state(State::DISCONNECTED);
+ server_connection_[node_index]->mutate_connected_since_time(
+ aos::monotonic_clock::min_time.time_since_epoch().count());
+}
+
void MessageBridgeServerStatus::SendStatistics() {
if (!send_) return;
aos::Sender<ServerStatistics>::Builder builder = sender_.MakeBuilder();
@@ -197,6 +220,17 @@
server_connection_builder.add_partial_deliveries(
partial_deliveries_[node_index]);
+ if (connection->connected_since_time() !=
+ monotonic_clock::min_time.time_since_epoch().count()) {
+ server_connection_builder.add_connected_since_time(
+ connection->connected_since_time());
+ }
+
+ if (connection->connection_count() != 0) {
+ server_connection_builder.add_connection_count(
+ connection->connection_count());
+ }
+
// TODO(austin): If it gets stale, drop it too.
if (!filters_[node_index].MissingSamples()) {
server_connection_builder.add_monotonic_offset(
diff --git a/aos/network/message_bridge_server_status.h b/aos/network/message_bridge_server_status.h
index 3a1c9ed..feb1b05 100644
--- a/aos/network/message_bridge_server_status.h
+++ b/aos/network/message_bridge_server_status.h
@@ -45,6 +45,9 @@
// Clears the boot UUID for the provided node.
void ClearBootUUID(int node_index);
+ void Connect(int node_index, monotonic_clock::time_point monotonic_now);
+ void Disconnect(int node_index);
+
// Returns the boot UUID for a node, or an empty string_view if there isn't
// one.
const UUID &BootUUID(int node_index) const { return boot_uuids_[node_index]; }
diff --git a/aos/network/message_bridge_test.cc b/aos/network/message_bridge_test.cc
index 8246c22..d2a9b05 100644
--- a/aos/network/message_bridge_test.cc
+++ b/aos/network/message_bridge_test.cc
@@ -439,7 +439,14 @@
pi2_client_event_loop->node()->name()->string_view()) {
if (connection->state() == State::CONNECTED) {
EXPECT_TRUE(connection->has_boot_uuid());
+ EXPECT_EQ(connection->connection_count(), 1u);
+ EXPECT_LT(monotonic_clock::time_point(chrono::nanoseconds(
+ connection->connected_since_time())),
+ monotonic_clock::now());
connected = true;
+ } else {
+ EXPECT_FALSE(connection->has_connection_count());
+ EXPECT_FALSE(connection->has_connected_since_time());
}
}
}
@@ -473,48 +480,95 @@
chrono::milliseconds(-1));
EXPECT_TRUE(connection->has_boot_uuid());
}
+
+ if (connection->state() == State::CONNECTED) {
+ EXPECT_EQ(connection->connection_count(), 1u);
+ EXPECT_LT(monotonic_clock::time_point(
+ chrono::nanoseconds(connection->connected_since_time())),
+ monotonic_clock::now());
+ } else {
+ EXPECT_FALSE(connection->has_connection_count());
+ EXPECT_FALSE(connection->has_connected_since_time());
+ }
}
});
int pi1_client_statistics_count = 0;
- ping_event_loop.MakeWatcher("/pi1/aos", [&pi1_client_statistics_count](
- const ClientStatistics &stats) {
- VLOG(1) << "/pi1/aos ClientStatistics " << FlatbufferToJson(&stats);
+ int pi1_connected_client_statistics_count = 0;
+ ping_event_loop.MakeWatcher(
+ "/pi1/aos",
+ [&pi1_client_statistics_count,
+ &pi1_connected_client_statistics_count](const ClientStatistics &stats) {
+ VLOG(1) << "/pi1/aos ClientStatistics " << FlatbufferToJson(&stats);
- for (const ClientConnection *connection : *stats.connections()) {
- if (connection->has_monotonic_offset()) {
- ++pi1_client_statistics_count;
- // It takes at least 10 microseconds to send a message between the
- // client and server. The min (filtered) time shouldn't be over 10
- // milliseconds on localhost. This might have to bump up if this is
- // proving flaky.
- EXPECT_LT(chrono::nanoseconds(connection->monotonic_offset()),
- chrono::milliseconds(10))
- << " " << connection->monotonic_offset()
- << "ns vs 10000ns on iteration " << pi1_client_statistics_count;
- EXPECT_GT(chrono::nanoseconds(connection->monotonic_offset()),
- chrono::microseconds(10))
- << " " << connection->monotonic_offset()
- << "ns vs 10000ns on iteration " << pi1_client_statistics_count;
- }
- }
- });
+ for (const ClientConnection *connection : *stats.connections()) {
+ if (connection->has_monotonic_offset()) {
+ ++pi1_client_statistics_count;
+ // It takes at least 10 microseconds to send a message between the
+ // client and server. The min (filtered) time shouldn't be over 10
+ // milliseconds on localhost. This might have to bump up if this is
+ // proving flaky.
+ EXPECT_LT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::milliseconds(10))
+ << " " << connection->monotonic_offset()
+ << "ns vs 10000ns on iteration " << pi1_client_statistics_count;
+ EXPECT_GT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::microseconds(10))
+ << " " << connection->monotonic_offset()
+ << "ns vs 10000ns on iteration " << pi1_client_statistics_count;
+ }
+ if (connection->state() == State::CONNECTED) {
+ EXPECT_EQ(connection->connection_count(), 1u);
+ EXPECT_LT(monotonic_clock::time_point(chrono::nanoseconds(
+ connection->connected_since_time())),
+ monotonic_clock::now());
+ // The first Connected message may not have a UUID in it since no
+ // data has flown. That's fine.
+ if (pi1_connected_client_statistics_count > 0) {
+ EXPECT_TRUE(connection->has_boot_uuid())
+ << ": " << aos::FlatbufferToJson(connection);
+ }
+ ++pi1_connected_client_statistics_count;
+ } else {
+ EXPECT_FALSE(connection->has_connection_count());
+ EXPECT_FALSE(connection->has_connected_since_time());
+ }
+ }
+ });
int pi2_client_statistics_count = 0;
- pong_event_loop.MakeWatcher("/pi2/aos", [&pi2_client_statistics_count](
- const ClientStatistics &stats) {
- VLOG(1) << "/pi2/aos ClientStatistics " << FlatbufferToJson(&stats);
+ int pi2_connected_client_statistics_count = 0;
+ pong_event_loop.MakeWatcher(
+ "/pi2/aos",
+ [&pi2_client_statistics_count,
+ &pi2_connected_client_statistics_count](const ClientStatistics &stats) {
+ VLOG(1) << "/pi2/aos ClientStatistics " << FlatbufferToJson(&stats);
- for (const ClientConnection *connection : *stats.connections()) {
- if (connection->has_monotonic_offset()) {
- ++pi2_client_statistics_count;
- EXPECT_LT(chrono::nanoseconds(connection->monotonic_offset()),
- chrono::milliseconds(10));
- EXPECT_GT(chrono::nanoseconds(connection->monotonic_offset()),
- chrono::microseconds(10));
- }
- }
- });
+ for (const ClientConnection *connection : *stats.connections()) {
+ if (connection->has_monotonic_offset()) {
+ ++pi2_client_statistics_count;
+ EXPECT_LT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::milliseconds(10))
+ << ": got " << aos::FlatbufferToJson(connection);
+ EXPECT_GT(chrono::nanoseconds(connection->monotonic_offset()),
+ chrono::microseconds(10))
+ << ": got " << aos::FlatbufferToJson(connection);
+ }
+ if (connection->state() == State::CONNECTED) {
+ EXPECT_EQ(connection->connection_count(), 1u);
+ EXPECT_LT(monotonic_clock::time_point(chrono::nanoseconds(
+ connection->connected_since_time())),
+ monotonic_clock::now());
+ if (pi2_connected_client_statistics_count > 0) {
+ EXPECT_TRUE(connection->has_boot_uuid());
+ }
+ ++pi2_connected_client_statistics_count;
+ } else {
+ EXPECT_FALSE(connection->has_connection_count());
+ EXPECT_FALSE(connection->has_connected_since_time());
+ }
+ }
+ });
ping_event_loop.MakeWatcher("/pi1/aos", [](const Timestamp ×tamp) {
EXPECT_TRUE(timestamp.has_offsets());
@@ -756,6 +810,8 @@
pi2_server_statistics_fetcher->connections()->Get(0);
EXPECT_EQ(pi1_connection->state(), State::CONNECTED);
+ EXPECT_EQ(pi1_connection->connection_count(), 1u);
+ EXPECT_TRUE(pi1_connection->has_connected_since_time());
EXPECT_TRUE(pi1_connection->has_monotonic_offset());
EXPECT_LT(chrono::nanoseconds(pi1_connection->monotonic_offset()),
chrono::milliseconds(1));
@@ -764,6 +820,8 @@
EXPECT_TRUE(pi1_connection->has_boot_uuid());
EXPECT_EQ(pi2_connection->state(), State::CONNECTED);
+ EXPECT_EQ(pi2_connection->connection_count(), 1u);
+ EXPECT_TRUE(pi2_connection->has_connected_since_time());
EXPECT_TRUE(pi2_connection->has_monotonic_offset());
EXPECT_LT(chrono::nanoseconds(pi2_connection->monotonic_offset()),
chrono::milliseconds(1));
@@ -787,11 +845,15 @@
pi2_server_statistics_fetcher->connections()->Get(0);
EXPECT_EQ(pi1_connection->state(), State::DISCONNECTED);
+ EXPECT_EQ(pi1_connection->connection_count(), 1u);
+ EXPECT_FALSE(pi1_connection->has_connected_since_time());
EXPECT_FALSE(pi1_connection->has_monotonic_offset());
EXPECT_FALSE(pi1_connection->has_boot_uuid());
EXPECT_EQ(pi2_connection->state(), State::CONNECTED);
EXPECT_FALSE(pi2_connection->has_monotonic_offset());
EXPECT_TRUE(pi2_connection->has_boot_uuid());
+ EXPECT_EQ(pi2_connection->connection_count(), 1u);
+ EXPECT_TRUE(pi2_connection->has_connected_since_time());
}
{
@@ -809,19 +871,27 @@
pi2_server_statistics_fetcher->connections()->Get(0);
EXPECT_EQ(pi1_connection->state(), State::CONNECTED);
+ EXPECT_EQ(pi1_connection->connection_count(), 2u);
+ EXPECT_TRUE(pi1_connection->has_connected_since_time());
EXPECT_TRUE(pi1_connection->has_monotonic_offset());
EXPECT_LT(chrono::nanoseconds(pi1_connection->monotonic_offset()),
- chrono::milliseconds(1));
+ chrono::milliseconds(1))
+ << ": " << FlatbufferToJson(pi1_connection);
EXPECT_GT(chrono::nanoseconds(pi1_connection->monotonic_offset()),
- chrono::milliseconds(-1));
+ chrono::milliseconds(-1))
+ << ": " << FlatbufferToJson(pi1_connection);
EXPECT_TRUE(pi1_connection->has_boot_uuid());
EXPECT_EQ(pi2_connection->state(), State::CONNECTED);
+ EXPECT_EQ(pi2_connection->connection_count(), 1u);
+ EXPECT_TRUE(pi2_connection->has_connected_since_time());
EXPECT_TRUE(pi2_connection->has_monotonic_offset());
EXPECT_LT(chrono::nanoseconds(pi2_connection->monotonic_offset()),
- chrono::milliseconds(1));
+ chrono::milliseconds(1))
+ << ": " << FlatbufferToJson(pi2_connection);
EXPECT_GT(chrono::nanoseconds(pi2_connection->monotonic_offset()),
- chrono::milliseconds(-1));
+ chrono::milliseconds(-1))
+ << ": " << FlatbufferToJson(pi2_connection);
EXPECT_TRUE(pi2_connection->has_boot_uuid());
StopPi2Client();
@@ -905,6 +975,8 @@
EXPECT_GT(chrono::nanoseconds(pi1_connection->monotonic_offset()),
chrono::milliseconds(-1));
EXPECT_TRUE(pi1_connection->has_boot_uuid());
+ EXPECT_TRUE(pi1_connection->has_connected_since_time());
+ EXPECT_EQ(pi1_connection->connection_count(), 1u);
EXPECT_EQ(pi2_connection->state(), State::CONNECTED);
EXPECT_TRUE(pi2_connection->has_monotonic_offset());
@@ -913,6 +985,8 @@
EXPECT_GT(chrono::nanoseconds(pi2_connection->monotonic_offset()),
chrono::milliseconds(-1));
EXPECT_TRUE(pi2_connection->has_boot_uuid());
+ EXPECT_TRUE(pi2_connection->has_connected_since_time());
+ EXPECT_EQ(pi2_connection->connection_count(), 1u);
StopPi2Server();
}
@@ -931,9 +1005,15 @@
EXPECT_EQ(pi1_server_connection->state(), State::CONNECTED);
EXPECT_FALSE(pi1_server_connection->has_monotonic_offset());
+ EXPECT_TRUE(pi1_server_connection->has_connected_since_time());
+ EXPECT_EQ(pi1_server_connection->connection_count(), 1u);
+
EXPECT_TRUE(pi1_server_connection->has_boot_uuid());
EXPECT_EQ(pi1_client_connection->state(), State::DISCONNECTED);
EXPECT_FALSE(pi1_client_connection->has_monotonic_offset());
+ EXPECT_FALSE(pi1_client_connection->has_connected_since_time());
+ EXPECT_EQ(pi1_client_connection->connection_count(), 1u);
+ EXPECT_FALSE(pi1_client_connection->has_boot_uuid());
}
{
@@ -944,11 +1024,14 @@
// And confirm we are synchronized again.
EXPECT_TRUE(pi1_server_statistics_fetcher.Fetch());
EXPECT_TRUE(pi2_server_statistics_fetcher.Fetch());
+ EXPECT_TRUE(pi1_client_statistics_fetcher.Fetch());
const ServerConnection *const pi1_connection =
pi1_server_statistics_fetcher->connections()->Get(0);
const ServerConnection *const pi2_connection =
pi2_server_statistics_fetcher->connections()->Get(0);
+ const ClientConnection *const pi1_client_connection =
+ pi1_client_statistics_fetcher->connections()->Get(0);
EXPECT_EQ(pi1_connection->state(), State::CONNECTED);
EXPECT_TRUE(pi1_connection->has_monotonic_offset());
@@ -958,6 +1041,11 @@
chrono::milliseconds(-1));
EXPECT_TRUE(pi1_connection->has_boot_uuid());
+ EXPECT_EQ(pi1_client_connection->state(), State::CONNECTED);
+ EXPECT_TRUE(pi1_client_connection->has_connected_since_time());
+ EXPECT_EQ(pi1_client_connection->connection_count(), 2u);
+ EXPECT_TRUE(pi1_client_connection->has_boot_uuid());
+
EXPECT_EQ(pi2_connection->state(), State::CONNECTED);
EXPECT_TRUE(pi2_connection->has_monotonic_offset());
EXPECT_LT(chrono::nanoseconds(pi2_connection->monotonic_offset()),
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 23a9d8e..53cc105 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -371,18 +371,6 @@
return ×_.back();
}
-void InterpolatedTimeConverter::QueueUntil(
- std::function<bool(const std::tuple<distributed_clock::time_point,
- std::vector<BootTimestamp>> &)>
- not_done) {
- while (!at_end_ && (times_.empty() || not_done(times_.back()))) {
- QueueNextTimestamp();
- }
-
- CHECK(!times_.empty())
- << ": Found no times to do timestamp estimation, please investigate.";
-}
-
void InterpolatedTimeConverter::ObserveTimePassed(
distributed_clock::time_point time) {
// Keep at least 500 points and time_estimation_buffer_seconds seconds of
diff --git a/aos/network/multinode_timestamp_filter.h b/aos/network/multinode_timestamp_filter.h
index 7de8041..d9c6f59 100644
--- a/aos/network/multinode_timestamp_filter.h
+++ b/aos/network/multinode_timestamp_filter.h
@@ -13,6 +13,7 @@
#include "aos/events/simulated_event_loop.h"
#include "aos/network/timestamp_filter.h"
#include "aos/time/time.h"
+#include "glog/logging.h"
namespace aos {
namespace message_bridge {
@@ -198,11 +199,15 @@
// Queues timestamps util the last time in the queue matches the provided
// function.
- void QueueUntil(
- std::function<
- bool(const std::tuple<distributed_clock::time_point,
- std::vector<logger::BootTimestamp>> &)>
- not_done);
+ template <typename F>
+ void QueueUntil(F not_done) {
+ while (!at_end_ && (times_.empty() || not_done(times_.back()))) {
+ QueueNextTimestamp();
+ }
+
+ CHECK(!times_.empty())
+ << ": Found no times to do timestamp estimation, please investigate.";
+ }
// The number of nodes to enforce.
const size_t node_count_;
diff --git a/aos/network/www/aos_plotter.ts b/aos/network/www/aos_plotter.ts
index 270c0d9..0145c07 100644
--- a/aos/network/www/aos_plotter.ts
+++ b/aos/network/www/aos_plotter.ts
@@ -190,7 +190,6 @@
private plots: AosPlot[] = [];
private messages = new Set<MessageHandler>();
- private lowestHeight = 0;
constructor(private readonly connection: Connection) {
// Set up to redraw at some regular interval. The exact rate is unimportant.
setInterval(() => {
@@ -223,24 +222,19 @@
// Add a new figure at the provided position with the provided size within
// parentElement.
addPlot(
- parentElement: Element, topLeft: number[]|null = null,
+ parentElement: Element,
size: number[] = [AosPlotter.DEFAULT_WIDTH, AosPlotter.DEFAULT_HEIGHT]):
AosPlot {
- if (topLeft === null) {
- topLeft = [0, this.lowestHeight];
- }
const div = document.createElement("div");
- div.style.top = topLeft[1].toString();
- div.style.left = topLeft[0].toString();
- div.style.position = 'absolute';
+ div.style.position = 'relative';
+ div.style.width = size[0].toString() + "px";
+ div.style.height = size[1].toString() + "px";
parentElement.appendChild(div);
- const newPlot = new Plot(div, size[0], size[1]);
+ const newPlot = new Plot(div);
for (let plot of this.plots.values()) {
newPlot.linkXAxis(plot.plot);
}
this.plots.push(new AosPlot(this, newPlot));
- // Height goes up as you go down.
- this.lowestHeight = Math.max(topLeft[1] + size[1], this.lowestHeight);
return this.plots[this.plots.length - 1];
}
private draw(): void {
diff --git a/aos/network/www/colors.ts b/aos/network/www/colors.ts
index b173e13..5c5dd86 100644
--- a/aos/network/www/colors.ts
+++ b/aos/network/www/colors.ts
@@ -1,4 +1,6 @@
export const RED: number[] = [1, 0, 0];
+export const ORANGE: number[] = [1, 0.65, 0];
+export const YELLOW: number[] = [1, 1, 0];
export const GREEN: number[] = [0, 1, 0];
export const BLUE: number[] = [0, 0, 1];
export const BROWN: number[] = [0.6, 0.3, 0];
diff --git a/aos/network/www/demo_plot.ts b/aos/network/www/demo_plot.ts
index cbd133a..ecd4da6 100644
--- a/aos/network/www/demo_plot.ts
+++ b/aos/network/www/demo_plot.ts
@@ -23,12 +23,12 @@
const height = AosPlotter.DEFAULT_HEIGHT;
const benchmarkDiv = document.createElement('div');
- benchmarkDiv.style.top = (height * 2).toString();
- benchmarkDiv.style.left = '0';
- benchmarkDiv.style.position = 'absolute';
+ benchmarkDiv.style.width = width.toString() + "px";
+ benchmarkDiv.style.height = height.toString() + "px";
+ benchmarkDiv.style.position = 'relative';
parentDiv.appendChild(benchmarkDiv);
- const benchmarkPlot = new Plot(benchmarkDiv, width, height);
+ const benchmarkPlot = new Plot(benchmarkDiv);
const aosPlotter = new AosPlotter(conn);
diff --git a/aos/network/www/plotter.ts b/aos/network/www/plotter.ts
index e56a808..842f1b7 100644
--- a/aos/network/www/plotter.ts
+++ b/aos/network/www/plotter.ts
@@ -409,7 +409,7 @@
public static readonly COLOR_CYCLE = [
Colors.RED, Colors.GREEN, Colors.BLUE, Colors.BROWN, Colors.PINK,
- Colors.CYAN, Colors.WHITE
+ Colors.CYAN, Colors.WHITE, Colors.ORANGE, Colors.YELLOW
];
private colorCycleIndex = 0;
@@ -850,6 +850,7 @@
export class Plot {
private canvas = document.createElement('canvas');
private textCanvas = document.createElement('canvas');
+ private lineDrawerContext: WebGLRenderingContext;
private drawer: LineDrawer;
private static keysPressed:
object = {'x': false, 'y': false, 'Escape': false};
@@ -869,25 +870,26 @@
private defaultYRange: number[]|null = null;
private zoomRectangle: Line;
- constructor(wrapperDiv: HTMLDivElement, width: number, height: number) {
+ constructor(wrapperDiv: HTMLDivElement) {
wrapperDiv.appendChild(this.canvas);
wrapperDiv.appendChild(this.textCanvas);
this.lastTimeMs = (new Date()).getTime();
- this.canvas.width =
- width - this.axisLabelBuffer.left - this.axisLabelBuffer.right;
- this.canvas.height =
- height - this.axisLabelBuffer.top - this.axisLabelBuffer.bottom;
- this.canvas.style.left = this.axisLabelBuffer.left.toString();
- this.canvas.style.top = this.axisLabelBuffer.top.toString();
- this.canvas.style.position = 'absolute';
- this.drawer = new LineDrawer(this.canvas.getContext('webgl'));
+ this.canvas.style.paddingLeft = this.axisLabelBuffer.left.toString() + "px";
+ this.canvas.style.paddingRight = this.axisLabelBuffer.right.toString() + "px";
+ this.canvas.style.paddingTop = this.axisLabelBuffer.top.toString() + "px";
+ this.canvas.style.paddingBottom = this.axisLabelBuffer.bottom.toString() + "px";
+ this.canvas.style.width = "100%";
+ this.canvas.style.height = "100%";
+ this.canvas.style.boxSizing = "border-box";
- this.textCanvas.width = width;
- this.textCanvas.height = height;
- this.textCanvas.style.left = '0';
- this.textCanvas.style.top = '0';
+ this.canvas.style.position = 'absolute';
+ this.lineDrawerContext = this.canvas.getContext('webgl');
+ this.drawer = new LineDrawer(this.lineDrawerContext);
+
this.textCanvas.style.position = 'absolute';
+ this.textCanvas.style.width = "100%";
+ this.textCanvas.style.height = "100%";
this.textCanvas.style.pointerEvents = 'none';
this.canvas.addEventListener('dblclick', (e) => {
@@ -934,9 +936,22 @@
}
mouseCanvasLocation(event: MouseEvent): number[] {
+ const computedStyle = window.getComputedStyle(this.canvas);
+ const paddingLeftStr = computedStyle.getPropertyValue('padding-left');
+ const paddingTopStr = computedStyle.getPropertyValue('padding-top');
+ if (paddingLeftStr.substring(paddingLeftStr.length - 2) != "px") {
+ throw new Error("Left padding should be specified in pixels.");
+ }
+ if (paddingTopStr.substring(paddingTopStr.length - 2) != "px") {
+ throw new Error("Left padding should be specified in pixels.");
+ }
+ // Javascript will just ignore the extra "px".
+ const paddingLeft = Number.parseInt(paddingLeftStr);
+ const paddingTop = Number.parseInt(paddingTopStr);
+
return [
- event.offsetX * 2.0 / this.canvas.width - 1.0,
- -event.offsetY * 2.0 / this.canvas.height + 1.0
+ (event.offsetX - paddingLeft) * 2.0 / this.canvas.width - 1.0,
+ -(event.offsetY - paddingTop) * 2.0 / this.canvas.height + 1.0
];
}
@@ -1154,6 +1169,17 @@
const curTime = (new Date()).getTime();
const frameRate = 1000.0 / (curTime - this.lastTimeMs);
this.lastTimeMs = curTime;
+ const parentWidth = this.textCanvas.parentElement.offsetWidth;
+ const parentHeight = this.textCanvas.parentElement.offsetHeight;
+ this.textCanvas.width = parentWidth;
+ this.textCanvas.height = parentHeight;
+ this.canvas.width =
+ parentWidth - this.axisLabelBuffer.left - this.axisLabelBuffer.right;
+ this.canvas.height =
+ parentHeight - this.axisLabelBuffer.top - this.axisLabelBuffer.bottom;
+ this.lineDrawerContext.viewport(
+ 0, 0, this.lineDrawerContext.drawingBufferWidth,
+ this.lineDrawerContext.drawingBufferHeight);
// Clear the overlay.
const textCtx = this.textCanvas.getContext("2d");
diff --git a/aos/time/time.cc b/aos/time/time.cc
index 882f925..a956f94 100644
--- a/aos/time/time.cc
+++ b/aos/time/time.cc
@@ -132,6 +132,9 @@
struct tm tm;
std::istringstream ss(std::string(now.substr(0, now.size() - 10)));
ss >> std::get_time(&tm, "%Y-%m-%d_%H-%M-%S");
+ if (ss.fail()) {
+ return std::nullopt;
+ }
tm.tm_isdst = -1;
time_t seconds = mktime(&tm);
diff --git a/aos/util/BUILD b/aos/util/BUILD
index 59dec18..daacba1 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -246,6 +246,7 @@
"//aos/scoped:scoped_fd",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/strings",
+ "@com_google_absl//absl/types:span",
],
)
diff --git a/aos/util/file.cc b/aos/util/file.cc
index a11e330..317206e 100644
--- a/aos/util/file.cc
+++ b/aos/util/file.cc
@@ -2,11 +2,15 @@
#include <fcntl.h>
#include <fts.h>
+#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <string_view>
+#if __has_feature(memory_sanitizer)
+#include <sanitizer/msan_interface.h>
+#endif
#include "aos/scoped/scoped_fd.h"
@@ -101,6 +105,15 @@
}
while ((curr = fts_read(ftsp))) {
+#if __has_feature(memory_sanitizer)
+ // fts_read doesn't have propper msan interceptors. Unpoison it ourselves.
+ if (curr) {
+ __msan_unpoison(curr, sizeof(*curr));
+ __msan_unpoison_string(curr->fts_accpath);
+ __msan_unpoison_string(curr->fts_path);
+ __msan_unpoison_string(curr->fts_name);
+ }
+#endif
switch (curr->fts_info) {
case FTS_NS:
case FTS_DNR:
@@ -139,5 +152,30 @@
}
}
+std::shared_ptr<absl::Span<uint8_t>> MMapFile(const std::string &path,
+ FileOptions options) {
+ int fd =
+ open(path.c_str(), options == FileOptions::kReadable ? O_RDONLY : O_RDWR);
+ PCHECK(fd != -1) << "Unable to open file " << path;
+ struct stat sb;
+ PCHECK(fstat(fd, &sb) != -1) << ": Unable to get file size of " << path;
+ uint8_t *start = reinterpret_cast<uint8_t *>(mmap(
+ NULL, sb.st_size,
+ options == FileOptions::kReadable ? PROT_READ : (PROT_READ | PROT_WRITE),
+ MAP_SHARED, fd, 0));
+ CHECK(start != MAP_FAILED) << ": Unable to open mapping to file " << path;
+ std::shared_ptr<absl::Span<uint8_t>> span =
+ std::shared_ptr<absl::Span<uint8_t>>(
+ new absl::Span<uint8_t>(start, sb.st_size),
+ [](absl::Span<uint8_t> *span) {
+ PCHECK(msync(span->data(), span->size(), MS_SYNC) == 0)
+ << ": Failed to flush data before unmapping.";
+ PCHECK(munmap(span->data(), span->size()) != -1);
+ delete span;
+ });
+ close(fd);
+ return span;
+}
+
} // namespace util
} // namespace aos
diff --git a/aos/util/file.h b/aos/util/file.h
index 8089225..2d37ec2 100644
--- a/aos/util/file.h
+++ b/aos/util/file.h
@@ -2,9 +2,12 @@
#define AOS_UTIL_FILE_H_
#include <sys/stat.h>
+
+#include <memory>
#include <string>
#include <string_view>
+#include "absl/types/span.h"
#include "glog/logging.h"
namespace aos {
@@ -32,6 +35,12 @@
// runs across.
void UnlinkRecursive(std::string_view path);
+enum class FileOptions { kReadable, kWriteable };
+
+// Maps file from disk into memory
+std::shared_ptr<absl::Span<uint8_t>> MMapFile(
+ const std::string &path, FileOptions options = FileOptions::kReadable);
+
} // namespace util
} // namespace aos
diff --git a/build_tests/BUILD b/build_tests/BUILD
index 0766256..6563297 100644
--- a/build_tests/BUILD
+++ b/build_tests/BUILD
@@ -1,5 +1,6 @@
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_py_library")
+load("@io_bazel_rules_go//go:def.bzl", "go_binary")
cc_test(
name = "gflags_build_test",
@@ -85,8 +86,6 @@
name = "python3_opencv",
srcs = ["python_opencv.py"],
main = "python_opencv.py",
- python_version = "PY3",
- srcs_version = "PY2AND3",
target_compatible_with = ["@platforms//os:linux"],
deps = ["@opencv_contrib_nonfree_amd64//:python_opencv"],
)
@@ -98,3 +97,9 @@
target_compatible_with = ["@platforms//os:linux"],
deps = ["@python_jinja2"],
)
+
+go_binary(
+ name = "hello_go",
+ srcs = ["hello.go"],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+)
diff --git a/build_tests/hello.go b/build_tests/hello.go
new file mode 100644
index 0000000..4cc584b
--- /dev/null
+++ b/build_tests/hello.go
@@ -0,0 +1,7 @@
+package main
+
+import "fmt"
+
+func main() {
+ fmt.Println("Hello world")
+}
diff --git a/debian/BUILD b/debian/BUILD
index 348163e..0ea9836 100644
--- a/debian/BUILD
+++ b/debian/BUILD
@@ -116,8 +116,6 @@
"download_packages.py",
],
main = "download_packages.py",
- python_version = "PY3",
- srcs_version = "PY2AND3",
target_compatible_with = ["@platforms//os:linux"],
)
diff --git a/debian/opencv_python.BUILD b/debian/opencv_python.BUILD
index 5afa180..5074bd3 100644
--- a/debian/opencv_python.BUILD
+++ b/debian/opencv_python.BUILD
@@ -5,6 +5,9 @@
include = ["**/*"],
exclude = ["**/*.py"],
),
+ deps = [
+ "@python_repo//:numpy",
+ ],
imports = ["."],
visibility = ["//visibility:public"],
)
diff --git a/debian/python.BUILD b/debian/python.BUILD
index 17543df..009a7e2 100644
--- a/debian/python.BUILD
+++ b/debian/python.BUILD
@@ -55,11 +55,45 @@
visibility = ["//visibility:public"],
)
-filegroup(
+py_library(
name = "scipy",
srcs = glob([
- "usr/lib/python3/dist-packages/numpy",
- "usr/lib/python3/dist-packages/scipy",
+ "usr/lib/python3/dist-packages/scipy/**/*.py",
+ ]),
+ data = glob([
+ "usr/lib/python3/dist-packages/scipy/**/*",
+ ], exclude = [
+ "usr/lib/python3/dist-packages/scipy/**/*.py",
+ ]),
+ deps = [
+ ":numpy",
+ ],
+ visibility = ["//visibility:public"],
+ imports = [
+ "usr/lib/python3/dist-packages",
+ ],
+ target_compatible_with = [
+ "@platforms//os:linux",
+ "@platforms//cpu:x86_64",
+ ],
+)
+
+py_library(
+ name = "numpy",
+ srcs = glob([
+ "usr/lib/python3/dist-packages/numpy/**/*.py",
+ ]),
+ data = glob([
+ "usr/lib/python3/dist-packages/numpy/**/*",
+ ], exclude = [
+ "usr/lib/python3/dist-packages/numpy/**/*.py",
]),
visibility = ["//visibility:public"],
+ imports = [
+ "usr/lib/python3/dist-packages",
+ ],
+ target_compatible_with = [
+ "@platforms//os:linux",
+ "@platforms//cpu:x86_64",
+ ],
)
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 5457405..4850f97 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -52,6 +52,7 @@
"//y2020/control_loops/superstructure:finisher_plotter",
"//y2020/control_loops/superstructure:hood_plotter",
"//y2020/control_loops/superstructure:turret_plotter",
+ "//y2021_bot3/control_loops/superstructure:superstructure_plotter",
],
)
diff --git a/frc971/analysis/in_process_plotter.cc b/frc971/analysis/in_process_plotter.cc
index c7a715a..0eaf719 100644
--- a/frc971/analysis/in_process_plotter.cc
+++ b/frc971/analysis/in_process_plotter.cc
@@ -25,6 +25,9 @@
color_wheel_.push_back(Color(1, 1, 0));
color_wheel_.push_back(Color(0, 1, 1));
color_wheel_.push_back(Color(1, 0, 1));
+ color_wheel_.push_back(Color(1, 0.6, 0));
+ color_wheel_.push_back(Color(0.6, 0.3, 0));
+ color_wheel_.push_back(Color(1, 1, 1));
}
void Plotter::Spin() { event_loop_factory_.Run(); }
diff --git a/frc971/analysis/plot_data_utils.ts b/frc971/analysis/plot_data_utils.ts
index def34ed..5d1f4a0 100644
--- a/frc971/analysis/plot_data_utils.ts
+++ b/frc971/analysis/plot_data_utils.ts
@@ -29,9 +29,6 @@
plotSelect.add(new Option('Select Plot', invalidSelectValue));
const plotDiv = document.createElement('div');
- plotDiv.style.position = 'absolute';
- plotDiv.style.top = '30';
- plotDiv.style.left = '0';
parentDiv.appendChild(plotDiv);
conn.addReliableHandler(
@@ -50,12 +47,11 @@
for (let ii = 0; ii < plotFb.figuresLength(); ++ii) {
const figure = plotFb.figures(ii);
const figureDiv = document.createElement('div');
- figureDiv.style.top = figure.position().top().toString();
- figureDiv.style.left = figure.position().left().toString();
- figureDiv.style.position = 'absolute';
+ figureDiv.style.width = figure.position().width().toString() + "px";
+ figureDiv.style.height = figure.position().height().toString() + "px";
+ figureDiv.style.position = 'relative';
div.appendChild(figureDiv);
- const plot = new Plot(
- figureDiv, figure.position().width(), figure.position().height());
+ const plot = new Plot(figureDiv);
if (figure.title()) {
plot.getAxisLabels().setTitle(figure.title());
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 7b46a70..2df65f6 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -38,6 +38,8 @@
'org_frc971/y2020/control_loops/superstructure/accelerator_plotter'
import {plotHood} from
'org_frc971/y2020/control_loops/superstructure/hood_plotter'
+import {plotSuperstructure} from
+ 'org_frc971/y2021_bot3/control_loops/superstructure/superstructure_plotter';
import {plotDemo} from 'org_frc971/aos/network/www/demo_plot';
import {plotData} from 'org_frc971/frc971/analysis/plot_data_utils';
@@ -104,6 +106,7 @@
['Turret', new PlotState(plotDiv, plotTurret)],
['2020 Localizer', new PlotState(plotDiv, plotLocalizer)],
['C++ Plotter', new PlotState(plotDiv, plotData)],
+ ['Y2021 3rd Robot Superstructure', new PlotState(plotDiv, plotSuperstructure)],
]);
const invalidSelectValue = 'null';
@@ -151,4 +154,4 @@
plotSelect.value = getDefaultPlot();
// Force the event to occur once at the start.
plotSelect.dispatchEvent(new Event('input'));
-});
+});
\ No newline at end of file
diff --git a/frc971/control_loops/drivetrain/down_estimator_plotter.ts b/frc971/control_loops/drivetrain/down_estimator_plotter.ts
index c6e414c..7f5bd58 100644
--- a/frc971/control_loops/drivetrain/down_estimator_plotter.ts
+++ b/frc971/control_loops/drivetrain/down_estimator_plotter.ts
@@ -18,7 +18,7 @@
'/drivetrain', 'frc971.IMUValuesBatch',
new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
- const accelPlot = aosPlotter.addPlot(element, [0, 0], [width, height]);
+ const accelPlot = aosPlotter.addPlot(element, [width, height]);
accelPlot.plot.getAxisLabels().setTitle(
'Estimated Accelerations (x = forward, y = lateral, z = vertical)');
accelPlot.plot.getAxisLabels().setYLabel('Acceleration (m/s/s)');
@@ -31,7 +31,7 @@
const accelZ = accelPlot.addMessageLine(status, ['down_estimator', 'accel_z']);
accelZ.setColor(BLUE);
- const velPlot = aosPlotter.addPlot(element, [0, height], [width, height]);
+ const velPlot = aosPlotter.addPlot(element, [width, height]);
velPlot.plot.getAxisLabels().setTitle('Raw IMU Integrated Velocity');
velPlot.plot.getAxisLabels().setYLabel('Velocity (m/s)');
velPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
@@ -43,7 +43,7 @@
const velZ = velPlot.addMessageLine(status, ['down_estimator', 'velocity_z']);
velZ.setColor(BLUE);
- const gravityPlot = aosPlotter.addPlot(element, [0, height * 2], [width, height]);
+ const gravityPlot = aosPlotter.addPlot(element, [width, height]);
gravityPlot.plot.getAxisLabels().setTitle('Accelerometer Magnitudes');
gravityPlot.plot.getAxisLabels().setXLabel('Monotonic Sent Time (sec)');
gravityPlot.plot.setDefaultYRange([0.95, 1.05]);
@@ -58,7 +58,7 @@
accelMagnitudeLine.setDrawLine(false);
const orientationPlot =
- aosPlotter.addPlot(element, [0, height * 3], [width, height]);
+ aosPlotter.addPlot(element, [width, height]);
orientationPlot.plot.getAxisLabels().setTitle('Orientation');
orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
@@ -75,7 +75,7 @@
yaw.setColor(BLUE);
yaw.setLabel('yaw');
- const imuAccelPlot = aosPlotter.addPlot(element, [0, height * 4], [width, height]);
+ const imuAccelPlot = aosPlotter.addPlot(element, [width, height]);
imuAccelPlot.plot.getAxisLabels().setTitle('Filtered Accelerometer Readings');
imuAccelPlot.plot.getAxisLabels().setYLabel('Acceleration (g)');
imuAccelPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
@@ -113,7 +113,7 @@
expectedAccelZ.setColor(BLUE);
expectedAccelZ.setPointSize(0);
- const gyroPlot = aosPlotter.addPlot(element, [0, height * 5], [width, height]);
+ const gyroPlot = aosPlotter.addPlot(element, [width, height]);
gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
@@ -141,7 +141,7 @@
const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
gyroZ.setColor(BLUE);
- const statePlot = aosPlotter.addPlot(element, [0, height * 6], [width, height / 2]);
+ const statePlot = aosPlotter.addPlot(element, [width, height / 2]);
statePlot.plot.getAxisLabels().setTitle('IMU State');
statePlot.plot.getAxisLabels().setXLabel('Monotonic Sent Time (sec)');
diff --git a/frc971/control_loops/drivetrain/drivetrain_plotter.ts b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
index f55f965..deb300f 100644
--- a/frc971/control_loops/drivetrain/drivetrain_plotter.ts
+++ b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
@@ -24,12 +24,9 @@
'/drivetrain', 'frc971.IMUValuesBatch',
new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
- let currentTop = 0;
-
// Polydrivetrain (teleop control) plots
const teleopPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- currentTop += DEFAULT_HEIGHT / 2;
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
teleopPlot.plot.getAxisLabels().setTitle('Drivetrain Teleop Goals');
teleopPlot.plot.getAxisLabels().setXLabel(TIME);
teleopPlot.plot.getAxisLabels().setYLabel('bool, throttle/wheel values');
@@ -44,8 +41,7 @@
// Drivetrain Control Mode
const modePlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- currentTop += DEFAULT_HEIGHT / 2;
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
// TODO(james): Actually add enum support.
modePlot.plot.getAxisLabels().setTitle(
'Drivetrain Mode [POLYDRIVE, MOTION_PROFILE, ' +
@@ -58,9 +54,7 @@
controllerType.setDrawLine(false);
// Drivetrain Status estimated relative position
- const positionPlot = aosPlotter.addPlot(element, [0, currentTop],
- [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const positionPlot = aosPlotter.addPlot(element);
positionPlot.plot.getAxisLabels().setTitle("Estimated Relative Position " +
"of the Drivetrain");
positionPlot.plot.getAxisLabels().setXLabel(TIME);
@@ -83,9 +77,7 @@
rightEncoder.setColor(CYAN);
// Drivetrain Output Voltage
- const outputPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const outputPlot = aosPlotter.addPlot(element);
outputPlot.plot.getAxisLabels().setTitle('Drivetrain Output');
outputPlot.plot.getAxisLabels().setXLabel(TIME);
outputPlot.plot.getAxisLabels().setYLabel('Voltage (V)');
@@ -96,9 +88,7 @@
rightVoltage.setColor(GREEN);
// Voltage Errors
- const voltageErrors =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const voltageErrors = aosPlotter.addPlot(element);
voltageErrors.plot.getAxisLabels().setTitle('Voltage Errors');
voltageErrors.plot.getAxisLabels().setXLabel(TIME);
voltageErrors.plot.getAxisLabels().setYLabel('Voltage (V)');
@@ -118,9 +108,7 @@
ekfRightVoltageError.setColor(CYAN);
// Sundry components of the output voltages
- const otherVoltages =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const otherVoltages = aosPlotter.addPlot(element);
otherVoltages.plot.getAxisLabels().setTitle('Other Voltage Components');
otherVoltages.plot.getAxisLabels().setXLabel(TIME);
otherVoltages.plot.getAxisLabels().setYLabel('Voltage (V)');
@@ -144,9 +132,7 @@
uncappedRightVoltage.setDrawLine(false);
// Drivetrain Velocities
- const velocityPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const velocityPlot = aosPlotter.addPlot(element);
velocityPlot.plot.getAxisLabels().setTitle('Velocity Plots');
velocityPlot.plot.getAxisLabels().setXLabel(TIME);
velocityPlot.plot.getAxisLabels().setYLabel('Wheel Velocity (m/s)');
@@ -183,9 +169,7 @@
rightSpeed.setColor(BROWN);
// Drivetrain trajectory and localizer velocities
- const velocityPlot2 = aosPlotter.addPlot(element, [0, currentTop],
- [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const velocityPlot2 = aosPlotter.addPlot(element);
velocityPlot2.plot.getAxisLabels().setTitle(
"Trajectory and Localizer Velocity Plots");
velocityPlot2.plot.getAxisLabels().setXLabel(TIME);
@@ -221,8 +205,7 @@
splineLateralVelocity.setPointSize(0.0);
// Heading
- const yawPlot = aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const yawPlot = aosPlotter.addPlot(element);
yawPlot.plot.getAxisLabels().setTitle('Robot Yaw');
yawPlot.plot.getAxisLabels().setXLabel(TIME);
yawPlot.plot.getAxisLabels().setYLabel('Yaw (rad)');
@@ -240,9 +223,7 @@
downEstimatorYaw.setColor(BLUE);
// Pitch/Roll
- const orientationPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const orientationPlot = aosPlotter.addPlot(element);
orientationPlot.plot.getAxisLabels().setTitle('Orientation');
orientationPlot.plot.getAxisLabels().setXLabel(TIME);
orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
@@ -257,9 +238,7 @@
pitch.setLabel('pitch');
// Accelerometer/Gravity
- const accelPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const accelPlot = aosPlotter.addPlot(element);
accelPlot.plot.getAxisLabels().setTitle('Accelerometer Readings');
accelPlot.plot.getAxisLabels().setYLabel('Acceleration (g)');
accelPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
@@ -293,9 +272,7 @@
accelZ.setDrawLine(false);
// Absolute X Position
- const xPositionPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const xPositionPlot = aosPlotter.addPlot(element);
xPositionPlot.plot.getAxisLabels().setTitle('X Position');
xPositionPlot.plot.getAxisLabels().setXLabel(TIME);
xPositionPlot.plot.getAxisLabels().setYLabel('X Position (m)');
@@ -307,9 +284,7 @@
splineX.setColor(GREEN);
// Absolute Y Position
- const yPositionPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const yPositionPlot = aosPlotter.addPlot(element);
yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
yPositionPlot.plot.getAxisLabels().setXLabel(TIME);
yPositionPlot.plot.getAxisLabels().setYLabel('Y Position (m)');
@@ -321,9 +296,7 @@
splineY.setColor(GREEN);
// Gyro
- const gyroPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const gyroPlot = aosPlotter.addPlot(element);
gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
@@ -353,8 +326,7 @@
// IMU States
const imuStatePlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- currentTop += DEFAULT_HEIGHT / 2;
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
imuStatePlot.plot.getAxisLabels().setTitle('IMU State');
imuStatePlot.plot.getAxisLabels().setXLabel(TIME);
imuStatePlot.plot.setDefaultYRange([-0.1, 1.1]);
diff --git a/frc971/control_loops/drivetrain/robot_state_plotter.ts b/frc971/control_loops/drivetrain/robot_state_plotter.ts
index 829df25..2ce8001 100644
--- a/frc971/control_loops/drivetrain/robot_state_plotter.ts
+++ b/frc971/control_loops/drivetrain/robot_state_plotter.ts
@@ -18,8 +18,7 @@
// Robot Enabled/Disabled and Mode
const robotStatePlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- currentTop += DEFAULT_HEIGHT / 2;
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
robotStatePlot.plot.getAxisLabels().setTitle('Robot State');
robotStatePlot.plot.getAxisLabels().setXLabel(TIME);
robotStatePlot.plot.getAxisLabels().setYLabel('bool');
@@ -41,7 +40,7 @@
// Battery Voltage
const batteryPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
currentTop += DEFAULT_HEIGHT / 2;
batteryPlot.plot.getAxisLabels().setTitle('Battery Voltage');
batteryPlot.plot.getAxisLabels().setXLabel(TIME);
@@ -51,7 +50,7 @@
// PID of process reading sensors
const readerPidPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
currentTop += DEFAULT_HEIGHT / 2;
readerPidPlot.plot.getAxisLabels().setTitle("PID of Process Reading Sensors");
readerPidPlot.plot.getAxisLabels().setXLabel(TIME);
diff --git a/frc971/control_loops/drivetrain/spline_plotter.ts b/frc971/control_loops/drivetrain/spline_plotter.ts
index 028a3fc..c39afd5 100644
--- a/frc971/control_loops/drivetrain/spline_plotter.ts
+++ b/frc971/control_loops/drivetrain/spline_plotter.ts
@@ -25,7 +25,7 @@
// Polydrivetrain (teleop control) plots
const longitudinalPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
currentTop += DEFAULT_HEIGHT / 2;
longitudinalPlot.plot.getAxisLabels().setTitle('Longitudinal Distance');
longitudinalPlot.plot.getAxisLabels().setXLabel(TIME);
@@ -35,7 +35,7 @@
status, ['trajectory_logging', 'distance_remaining']);
const boolPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
currentTop += DEFAULT_HEIGHT / 2;
boolPlot.plot.getAxisLabels().setTitle('Bool Flags');
boolPlot.plot.getAxisLabels().setXLabel(TIME);
@@ -46,8 +46,7 @@
boolPlot.addMessageLine(status, ['trajectory_logging', 'is_executed'])
.setColor(BLUE);
- const handlePlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ const handlePlot = aosPlotter.addPlot(element);
currentTop += DEFAULT_HEIGHT;
handlePlot.plot.getAxisLabels().setTitle('Spline Handles');
handlePlot.plot.getAxisLabels().setXLabel(TIME);
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index a4b8fb8..8679374 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -26,12 +26,12 @@
],
data = [
"//third_party/cddlib:_cddlib.so",
- "@python_repo//:scipy",
],
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
":python_init",
"//external:python-glog",
+ "@python_repo//:scipy",
],
)
@@ -98,6 +98,7 @@
deps = [
":libspline",
":python_init",
+ "@python_repo//:numpy",
],
)
diff --git a/frc971/wpilib/imu_plotter.ts b/frc971/wpilib/imu_plotter.ts
index af23ed9..0c735eb 100644
--- a/frc971/wpilib/imu_plotter.ts
+++ b/frc971/wpilib/imu_plotter.ts
@@ -10,7 +10,7 @@
const height = 400;
const aosPlotter = new AosPlotter(conn);
- const accelPlot = aosPlotter.addPlot(element, [0, 0], [width, height]);
+ const accelPlot = aosPlotter.addPlot(element, [width, height]);
accelPlot.plot.getAxisLabels().setTitle('Accelerometer Readings');
accelPlot.plot.getAxisLabels().setYLabel('Acceleration (g)');
accelPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
@@ -29,7 +29,7 @@
const accelZ = accelPlot.addMessageLine(imu, ['accelerometer_z']);
accelZ.setColor([0, 0, 1]);
- const gyroPlot = aosPlotter.addPlot(element, [0, height], [width, height]);
+ const gyroPlot = aosPlotter.addPlot(element, [width, height]);
gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
@@ -57,14 +57,14 @@
const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
gyroZ.setColor([0, 0, 1]);
- const tempPlot = aosPlotter.addPlot(element, [0, height * 2], [width, height / 2]);
+ const tempPlot = aosPlotter.addPlot(element, [width, height / 2]);
tempPlot.plot.getAxisLabels().setTitle('IMU Temperature');
tempPlot.plot.getAxisLabels().setYLabel('Temperature (deg C)');
tempPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
tempPlot.addMessageLine(imu, ['temperature']);
- const statePlot = aosPlotter.addPlot(element, [0, height * 2.5], [width, height / 2]);
+ const statePlot = aosPlotter.addPlot(element, [width, height / 2]);
statePlot.plot.getAxisLabels().setTitle('IMU State');
statePlot.plot.getAxisLabels().setXLabel('Monotonic Sent Time (sec)');
statePlot.plot.setDefaultYRange([-0.1, 1.1]);
diff --git a/motors/fet12/BUILD b/motors/fet12/BUILD
index ac9849b..be744b2 100644
--- a/motors/fet12/BUILD
+++ b/motors/fet12/BUILD
@@ -84,7 +84,7 @@
srcs = [
"calib_sensors.py",
],
- data = [
+ deps = [
"@python_repo//:scipy",
],
target_compatible_with = ["@platforms//os:linux"],
@@ -95,7 +95,7 @@
srcs = [
"current_equalize.py",
],
- data = [
+ deps = [
":calib_sensors",
"@python_repo//:scipy",
],
diff --git a/motors/python/BUILD b/motors/python/BUILD
index 90ab608..2b2b80b 100644
--- a/motors/python/BUILD
+++ b/motors/python/BUILD
@@ -3,9 +3,6 @@
srcs = [
"big_phase_current.py",
],
- data = [
- "@python_repo//:scipy",
- ],
legacy_create_init = False,
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
@@ -14,6 +11,7 @@
"//external:python-glog",
"//frc971/control_loops/python:controls",
"@matplotlib_repo//:matplotlib3",
+ "@python_repo//:scipy",
],
)
diff --git a/third_party/googletest/googlemock/test/BUILD.bazel b/third_party/googletest/googlemock/test/BUILD.bazel
index 2c3f5dc..882a423 100644
--- a/third_party/googletest/googlemock/test/BUILD.bazel
+++ b/third_party/googletest/googlemock/test/BUILD.bazel
@@ -107,7 +107,6 @@
":gmock_output_test_",
":gmock_output_test_golden.txt",
],
- python_version = "PY2",
tags = [
"no_test_msvc2015",
"no_test_msvc2017",
diff --git a/tools/bazel b/tools/bazel
index ef6372b..e72b44e 100755
--- a/tools/bazel
+++ b/tools/bazel
@@ -24,7 +24,7 @@
exec "${BAZEL_OVERRIDE}" "$@"
fi
-readonly VERSION="4.1.0"
+readonly VERSION="4.2.2"
readonly DOWNLOAD_DIR="${HOME}/.cache/bazel"
# Directory to unpack bazel into. This must change whenever bazel changes.
diff --git a/tools/go/BUILD b/tools/go/BUILD
new file mode 100644
index 0000000..ec8aa26
--- /dev/null
+++ b/tools/go/BUILD
@@ -0,0 +1,53 @@
+# This file exists to create a NOOP toolchain for Go on platforms that don't
+# support Go. We can probably get rid of this once
+# https://github.com/bazelbuild/bazel/issues/12897 is fixed.
+#
+# For platforms that do support Go, we use go_register_toolchain() in
+# WORKSPACE.
+
+load("@bazel_skylib//rules:write_file.bzl", "write_file")
+load("@io_bazel_rules_go//go:def.bzl", "go_sdk", "go_toolchain")
+
+write_file(
+ name = "noop_error_exit",
+ out = "noop_error_exit.sh",
+ content = [
+ "#!/bin/bash",
+ "echo 'This should never be executed. Something went wrong.' >&2",
+ "echo 'This NOOP Go toolchain should never be executed. Something went wrong.' >&2",
+ "echo 'Check that your target has `target_compatible_with` set to a platform that supports Go.' >&2",
+ "exit 1",
+ ],
+ is_executable = True,
+)
+
+go_sdk(
+ name = "noop_sdk",
+ go = ":noop_error_exit",
+ goarch = "none",
+ goos = "none",
+ root_file = "NOOP_FILE_THAT_DOES_NOT_EXIST",
+)
+
+go_toolchain(
+ name = "noop_go_toolchain_impl",
+ builder = ":noop_error_exit",
+ cgo_link_flags = None,
+ goarch = "none",
+ goos = "none",
+ link_flags = None,
+ sdk = ":noop_sdk",
+ tags = ["manual"],
+)
+
+toolchain(
+ name = "noop_go_toolchain",
+ exec_compatible_with = [
+ "@platforms//os:linux",
+ ],
+ target_compatible_with = [
+ "//tools/platforms/go:lacks_support",
+ ],
+ toolchain = ":noop_go_toolchain_impl",
+ toolchain_type = "@io_bazel_rules_go//go:toolchain",
+)
diff --git a/tools/platforms/BUILD b/tools/platforms/BUILD
index 65563bf..4aa3db3 100644
--- a/tools/platforms/BUILD
+++ b/tools/platforms/BUILD
@@ -5,6 +5,7 @@
constraint_values = [
"@platforms//os:linux",
"@platforms//cpu:x86_64",
+ "//tools/platforms/go:has_support",
],
)
@@ -14,6 +15,7 @@
"@platforms//os:linux",
"@platforms//cpu:armv7",
"//tools/platforms/hardware:raspberry_pi",
+ "//tools/platforms/go:lacks_support",
],
)
@@ -22,6 +24,7 @@
constraint_values = [
"@platforms//os:linux",
"@platforms//cpu:arm64",
+ "//tools/platforms/go:lacks_support",
],
)
@@ -31,6 +34,7 @@
"@platforms//os:linux",
"@platforms//cpu:armv7",
"//tools/platforms/hardware:roborio",
+ "//tools/platforms/go:lacks_support",
],
)
@@ -39,6 +43,7 @@
constraint_values = [
"@platforms//os:none",
"//tools/platforms/hardware:cortex_m4f",
+ "//tools/platforms/go:lacks_support",
],
)
diff --git a/tools/platforms/go/BUILD b/tools/platforms/go/BUILD
new file mode 100644
index 0000000..5936287
--- /dev/null
+++ b/tools/platforms/go/BUILD
@@ -0,0 +1,13 @@
+package(default_visibility = ["//visibility:public"])
+
+constraint_setting(name = "go_support")
+
+constraint_value(
+ name = "has_support",
+ constraint_setting = ":go_support",
+)
+
+constraint_value(
+ name = "lacks_support",
+ constraint_setting = ":go_support",
+)
diff --git a/tools/python/BUILD b/tools/python/BUILD
index 2181b31..32093d3 100644
--- a/tools/python/BUILD
+++ b/tools/python/BUILD
@@ -1,9 +1,31 @@
+load("@rules_python//python:defs.bzl", "py_runtime_pair")
+
py_runtime(
- name = "runtime",
+ name = "python3_runtime",
files = [
"runtime_binary.sh",
"@python_repo//:all_files",
],
interpreter = "runtime_binary.sh",
- visibility = ["//visibility:public"],
+ python_version = "PY3",
+)
+
+py_runtime_pair(
+ name = "py_runtime",
+ py2_runtime = None,
+ py3_runtime = ":python3_runtime",
+)
+
+toolchain(
+ name = "python_toolchain",
+ target_compatible_with = [
+ "@platforms//cpu:x86_64",
+ "@platforms//os:linux",
+ ],
+ exec_compatible_with = [
+ "@platforms//cpu:x86_64",
+ "@platforms//os:linux",
+ ],
+ toolchain = ":py_runtime",
+ toolchain_type = "@rules_python//python:toolchain_type",
)
diff --git a/tools/python/runtime_binary.sh b/tools/python/runtime_binary.sh
index 8380424..9cd1519 100755
--- a/tools/python/runtime_binary.sh
+++ b/tools/python/runtime_binary.sh
@@ -36,4 +36,5 @@
export LD_LIBRARY_PATH="${BASE_PATH}/usr/lib/lapack:${BASE_PATH}/usr/lib/libblas:${BASE_PATH}/usr/lib/x86_64-linux-gnu"
-exec "$BASE_PATH"/usr/bin/python3 "$@"
+# Prevent Python from importing the host's installed packages.
+exec "$BASE_PATH"/usr/bin/python3 -sS "$@"
diff --git a/y2012/BUILD b/y2012/BUILD
deleted file mode 100644
index ddcdb43..0000000
--- a/y2012/BUILD
+++ /dev/null
@@ -1,77 +0,0 @@
-load("//frc971:downloader.bzl", "robot_downloader")
-
-cc_binary(
- name = "joystick_reader",
- srcs = [
- "joystick_reader.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- "//aos:init",
- "//aos/actions:action_lib",
- "//aos/logging",
- "//aos/time",
- "//aos/util:log_interval",
- "//frc971/autonomous:auto_fbs",
- "//frc971/control_loops:control_loops_fbs",
- "//frc971/control_loops/drivetrain:drivetrain_goal_fbs",
- "//frc971/control_loops/drivetrain:spline_goal_fbs",
- "//frc971/input:joystick_input",
- "//y2012/control_loops/accessories:accessories_fbs",
- ],
-)
-
-robot_downloader(
- start_binaries = [
- ":joystick_reader",
- ":wpilib_interface",
- "//y2012/control_loops/drivetrain",
- "//y2012/control_loops/accessories",
- ],
- target_compatible_with = ["@platforms//os:linux"],
-)
-
-cc_binary(
- name = "wpilib_interface",
- srcs = [
- "wpilib_interface.cc",
- ],
- target_compatible_with = ["//tools/platforms/hardware:roborio"],
- deps = [
- "//aos:init",
- "//aos/events:shm_event_loop",
- "//aos/logging",
- "//aos/stl_mutex",
- "//aos/time",
- "//aos/util:log_interval",
- "//aos/util:phased_loop",
- "//aos/util:wrapping_counter",
- "//frc971/control_loops:control_loop",
- "//frc971/control_loops:control_loops_fbs",
- "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
- "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
- "//frc971/input:robot_state_fbs",
- "//frc971/wpilib:buffered_pcm",
- "//frc971/wpilib:dma",
- "//frc971/wpilib:dma_edge_counting",
- "//frc971/wpilib:drivetrain_writer",
- "//frc971/wpilib:encoder_and_potentiometer",
- "//frc971/wpilib:interrupt_edge_counting",
- "//frc971/wpilib:joystick_sender",
- "//frc971/wpilib:logging_fbs",
- "//frc971/wpilib:loop_output_handler",
- "//frc971/wpilib:sensor_reader",
- "//frc971/wpilib:wpilib_interface",
- "//frc971/wpilib:wpilib_robot_base",
- "//third_party:wpilib",
- "//y2012/control_loops:control_loop_fbs",
- "//y2012/control_loops/accessories:accessories_fbs",
- ],
-)
-
-py_library(
- name = "python_init",
- srcs = ["__init__.py"],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
-)
diff --git a/y2012/__init__.py b/y2012/__init__.py
deleted file mode 100644
index e69de29..0000000
--- a/y2012/__init__.py
+++ /dev/null
diff --git a/y2012/control_loops/BUILD b/y2012/control_loops/BUILD
deleted file mode 100644
index ef4e03f..0000000
--- a/y2012/control_loops/BUILD
+++ /dev/null
@@ -1,18 +0,0 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-
-py_library(
- name = "python_init",
- srcs = ["__init__.py"],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = ["//y2012:python_init"],
-)
-
-flatbuffer_cc_library(
- name = "control_loop_fbs",
- srcs = [
- "control_loops.fbs",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2012:__subpackages__"],
-)
diff --git a/y2012/control_loops/__init__.py b/y2012/control_loops/__init__.py
deleted file mode 100644
index e69de29..0000000
--- a/y2012/control_loops/__init__.py
+++ /dev/null
diff --git a/y2012/control_loops/accessories/BUILD b/y2012/control_loops/accessories/BUILD
deleted file mode 100644
index ee61511..0000000
--- a/y2012/control_loops/accessories/BUILD
+++ /dev/null
@@ -1,25 +0,0 @@
-package(default_visibility = ["//visibility:public"])
-
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-
-cc_binary(
- name = "accessories",
- srcs = [
- "accessories.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":accessories_fbs",
- "//aos:init",
- "//frc971/control_loops:control_loop",
- "//y2012/control_loops:control_loop_fbs",
- ],
-)
-
-flatbuffer_cc_library(
- name = "accessories_fbs",
- srcs = [
- "accessories.fbs",
- ],
- target_compatible_with = ["@platforms//os:linux"],
-)
diff --git a/y2012/control_loops/accessories/accessories.cc b/y2012/control_loops/accessories/accessories.cc
deleted file mode 100644
index c71146e..0000000
--- a/y2012/control_loops/accessories/accessories.cc
+++ /dev/null
@@ -1,60 +0,0 @@
-#include "aos/events/shm_event_loop.h"
-#include "aos/init.h"
-#include "frc971/control_loops/control_loop.h"
-#include "y2012/control_loops/accessories/accessories_generated.h"
-#include "y2012/control_loops/control_loops_generated.h"
-
-namespace y2012 {
-namespace control_loops {
-namespace accessories {
-
-class AccessoriesLoop : public ::frc971::controls::ControlLoop<
- Message, ::aos::control_loops::Position,
- ::aos::control_loops::Status, Message> {
- public:
- explicit AccessoriesLoop(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2012.control_loops.accessories_queue")
- : ::frc971::controls::ControlLoop<Message, ::aos::control_loops::Position,
- ::aos::control_loops::Status, Message>(
- event_loop, name) {}
-
- void RunIteration(const Message *goal,
- const ::aos::control_loops::Position * /*position*/,
- ::aos::Sender<Message>::Builder *output,
- ::aos::Sender<::aos::control_loops::Status>::Builder
- * /*status*/) override {
- if (output) {
- //*output = *goal;
- Message::Builder output_builder = output->MakeBuilder<Message>();
- flatbuffers::Offset<flatbuffers::Vector<uint8_t>> solenoid_offset =
- output->fbb()->template CreateVector<uint8_t>(
- goal->solenoids()->data(), 3);
- output_builder.add_solenoids(solenoid_offset);
- flatbuffers::Offset<flatbuffers::Vector<double>> stick_offset =
- output->fbb()->template CreateVector<double>(goal->sticks()->data(),
- 2);
- output_builder.add_sticks(stick_offset);
-
- output_builder.Finish();
- }
- }
-};
-
-} // namespace accessories
-} // namespace control_loops
-} // namespace y2012
-
-int main(int argc, char **argv) {
- ::aos::InitGoogle(&argc, &argv);
-
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig("config.json");
-
- ::aos::ShmEventLoop event_loop(&config.message());
- ::y2012::control_loops::accessories::AccessoriesLoop accessories(&event_loop);
-
- event_loop.Run();
-
- return 0;
-}
diff --git a/y2012/control_loops/accessories/accessories.fbs b/y2012/control_loops/accessories/accessories.fbs
deleted file mode 100644
index 0840da4..0000000
--- a/y2012/control_loops/accessories/accessories.fbs
+++ /dev/null
@@ -1,8 +0,0 @@
-namespace y2012.control_loops.accessories;
-
-table Message {
- solenoids:[bool] (id: 0); // Exactly 3 values
- sticks:[double] (id: 1); // Exactly 2 values
-}
-
-root_type Message;
diff --git a/y2012/control_loops/control_loops.fbs b/y2012/control_loops/control_loops.fbs
deleted file mode 100644
index be5b7db..0000000
--- a/y2012/control_loops/control_loops.fbs
+++ /dev/null
@@ -1,24 +0,0 @@
-namespace aos.control_loops;
-// This file defines basic messages for a Single Input Single Output (SISO)
-// control loop.
-
-table Goal {
- goal:double (id: 0);
-}
-
-table Position {
- position:double (id: 0);
-}
-
-table Output {
- voltage:double (id: 0);
-}
-
-table Status {
- done:bool (id: 0);
-}
-
-root_type Goal;
-root_type Position;
-root_type Output;
-root_type Status;
diff --git a/y2012/control_loops/drivetrain/BUILD b/y2012/control_loops/drivetrain/BUILD
deleted file mode 100644
index cd575ea..0000000
--- a/y2012/control_loops/drivetrain/BUILD
+++ /dev/null
@@ -1,87 +0,0 @@
-package(default_visibility = ["//visibility:public"])
-
-genrule(
- name = "genrule_drivetrain",
- outs = [
- "drivetrain_dog_motor_plant.h",
- "drivetrain_dog_motor_plant.cc",
- "kalman_drivetrain_motor_plant.h",
- "kalman_drivetrain_motor_plant.cc",
- ],
- cmd = "$(location //y2012/control_loops/python:drivetrain) $(OUTS)",
- target_compatible_with = ["@platforms//os:linux"],
- tools = [
- "//y2012/control_loops/python:drivetrain",
- ],
- visibility = ["//visibility:private"],
-)
-
-genrule(
- name = "genrule_polydrivetrain",
- outs = [
- "polydrivetrain_dog_motor_plant.h",
- "polydrivetrain_dog_motor_plant.cc",
- "polydrivetrain_cim_plant.h",
- "polydrivetrain_cim_plant.cc",
- "hybrid_velocity_drivetrain.h",
- "hybrid_velocity_drivetrain.cc",
- ],
- cmd = "$(location //y2012/control_loops/python:polydrivetrain) $(OUTS)",
- target_compatible_with = ["@platforms//os:linux"],
- tools = [
- "//y2012/control_loops/python:polydrivetrain",
- ],
- visibility = ["//visibility:private"],
-)
-
-cc_library(
- name = "polydrivetrain_plants",
- srcs = [
- "drivetrain_dog_motor_plant.cc",
- "hybrid_velocity_drivetrain.cc",
- "kalman_drivetrain_motor_plant.cc",
- "polydrivetrain_dog_motor_plant.cc",
- ],
- hdrs = [
- "drivetrain_dog_motor_plant.h",
- "hybrid_velocity_drivetrain.h",
- "kalman_drivetrain_motor_plant.h",
- "polydrivetrain_dog_motor_plant.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- "//frc971/control_loops:hybrid_state_feedback_loop",
- "//frc971/control_loops:state_feedback_loop",
- ],
-)
-
-cc_library(
- name = "drivetrain_base",
- srcs = [
- "drivetrain_base.cc",
- ],
- hdrs = [
- "drivetrain_base.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":polydrivetrain_plants",
- "//frc971:shifter_hall_effect",
- "//frc971/control_loops/drivetrain:drivetrain_config",
- "//y2016:constants",
- ],
-)
-
-cc_binary(
- name = "drivetrain",
- srcs = [
- "drivetrain_main.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":drivetrain_base",
- "//aos:init",
- "//aos/events:shm_event_loop",
- "//frc971/control_loops/drivetrain:drivetrain_lib",
- ],
-)
diff --git a/y2012/control_loops/drivetrain/drivetrain_base.cc b/y2012/control_loops/drivetrain/drivetrain_base.cc
deleted file mode 100644
index 40f4d24..0000000
--- a/y2012/control_loops/drivetrain/drivetrain_base.cc
+++ /dev/null
@@ -1,59 +0,0 @@
-#include "y2012/control_loops/drivetrain/drivetrain_base.h"
-
-#include <chrono>
-
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2012/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2012/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
-#include "y2012/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
-#include "y2012/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
-
-using ::frc971::control_loops::drivetrain::DrivetrainConfig;
-
-namespace chrono = ::std::chrono;
-
-namespace y2012 {
-namespace control_loops {
-namespace drivetrain {
-
-using ::frc971::constants::ShifterHallEffect;
-
-const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
-
-const DrivetrainConfig<double> &GetDrivetrainConfig() {
- static DrivetrainConfig<double> kDrivetrainConfig{
- ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
- ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
- ::frc971::control_loops::drivetrain::IMUType::IMU_X,
-
- drivetrain::MakeDrivetrainLoop,
- drivetrain::MakeVelocityDrivetrainLoop,
- drivetrain::MakeKFDrivetrainLoop,
- drivetrain::MakeHybridVelocityDrivetrainLoop,
-
- chrono::duration_cast<chrono::nanoseconds>(
- chrono::duration<double>(drivetrain::kDt)),
- drivetrain::kRobotRadius,
- drivetrain::kWheelRadius,
- drivetrain::kV,
-
- drivetrain::kHighGearRatio,
- drivetrain::kLowGearRatio,
- drivetrain::kJ,
- drivetrain::kMass,
- kThreeStateDriveShifter,
- kThreeStateDriveShifter,
- true /* default_high_gear */,
- 0.0,
- 0.4 /* wheel_non_linearity */,
- 1.0 /* quickturn_wheel_multiplier */,
- 1.0 /* wheel_multiplier */};
-
- return kDrivetrainConfig;
-};
-
-} // namespace drivetrain
-} // namespace control_loops
-} // namespace y2012
diff --git a/y2012/control_loops/drivetrain/drivetrain_base.h b/y2012/control_loops/drivetrain/drivetrain_base.h
deleted file mode 100644
index f5675a8..0000000
--- a/y2012/control_loops/drivetrain/drivetrain_base.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#ifndef Y2012_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
-#define Y2012_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
-
-#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-
-namespace y2012 {
-namespace control_loops {
-namespace drivetrain {
-
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
-
-} // namespace drivetrain
-} // namespace control_loops
-} // namespace y2012
-
-#endif // Y2012_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2012/control_loops/drivetrain/drivetrain_main.cc b/y2012/control_loops/drivetrain/drivetrain_main.cc
deleted file mode 100644
index 8665e91..0000000
--- a/y2012/control_loops/drivetrain/drivetrain_main.cc
+++ /dev/null
@@ -1,26 +0,0 @@
-#include <memory>
-
-#include "aos/events/shm_event_loop.h"
-#include "aos/init.h"
-#include "frc971/control_loops/drivetrain/drivetrain.h"
-#include "y2012/control_loops/drivetrain/drivetrain_base.h"
-
-using ::frc971::control_loops::drivetrain::DrivetrainLoop;
-
-int main(int argc, char **argv) {
- ::aos::InitGoogle(&argc, &argv);
-
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig("config.json");
-
- ::aos::ShmEventLoop event_loop(&config.message());
- ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
- &event_loop, ::y2012::control_loops::drivetrain::GetDrivetrainConfig());
- std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
- ::y2012::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
- &localizer);
-
- event_loop.Run();
-
- return 0;
-}
diff --git a/y2012/control_loops/python/BUILD b/y2012/control_loops/python/BUILD
deleted file mode 100644
index 1bceece..0000000
--- a/y2012/control_loops/python/BUILD
+++ /dev/null
@@ -1,54 +0,0 @@
-package(default_visibility = ["//y2012:__subpackages__"])
-
-py_binary(
- name = "drivetrain",
- srcs = [
- "drivetrain.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:drivetrain",
- ],
-)
-
-py_binary(
- name = "polydrivetrain",
- srcs = [
- "drivetrain.py",
- "polydrivetrain.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:polydrivetrain",
- ],
-)
-
-py_library(
- name = "polydrivetrain_lib",
- srcs = [
- "drivetrain.py",
- "polydrivetrain.py",
- ],
- target_compatible_with = ["@platforms//cpu:x86_64"],
- deps = [
- "//external:python-gflags",
- "//external:python-glog",
- "//frc971/control_loops/python:controls",
- ],
-)
-
-py_library(
- name = "python_init",
- srcs = ["__init__.py"],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = ["//y2012/control_loops:python_init"],
-)
diff --git a/y2012/control_loops/python/__init__.py b/y2012/control_loops/python/__init__.py
deleted file mode 100644
index e69de29..0000000
--- a/y2012/control_loops/python/__init__.py
+++ /dev/null
diff --git a/y2012/control_loops/python/drivetrain.py b/y2012/control_loops/python/drivetrain.py
deleted file mode 100755
index 5449503..0000000
--- a/y2012/control_loops/python/drivetrain.py
+++ /dev/null
@@ -1,41 +0,0 @@
-#!/usr/bin/python3
-
-from __future__ import print_function
-from frc971.control_loops.python import drivetrain
-import sys
-
-import gflags
-import glog
-
-FLAGS = gflags.FLAGS
-
-gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-
-kDrivetrain = drivetrain.DrivetrainParams(J = 1.5,
- mass = 30,
- robot_radius = 0.647998644 / 2.0,
- wheel_radius = .04445,
- G_high = 30.0 / 45.0 * 15.0 / 50.0,
- G_low = 15.0 / 60.0 * 15.0 / 50.0,
- q_pos_low = 0.12,
- q_pos_high = 0.14,
- q_vel_low = 1.0,
- q_vel_high = 0.95,
- has_imu = False,
- dt = 0.005,
- controller_poles = [0.8, 0.8])
-
-def main(argv):
- argv = FLAGS(argv)
- glog.init()
-
- if FLAGS.plot:
- drivetrain.PlotDrivetrainMotions(kDrivetrain)
- elif len(argv) != 5:
- print("Expected .h file name and .cc file name")
- else:
- # Write the generated constants out to a file.
- drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2012', kDrivetrain)
-
-if __name__ == '__main__':
- sys.exit(main(sys.argv))
diff --git a/y2012/control_loops/python/polydrivetrain.py b/y2012/control_loops/python/polydrivetrain.py
deleted file mode 100755
index ae26dbe..0000000
--- a/y2012/control_loops/python/polydrivetrain.py
+++ /dev/null
@@ -1,31 +0,0 @@
-#!/usr/bin/python3
-
-import sys
-from y2012.control_loops.python import drivetrain
-from frc971.control_loops.python import polydrivetrain
-
-import gflags
-import glog
-
-__author__ = 'Austin Schuh (austin.linux@gmail.com)'
-
-FLAGS = gflags.FLAGS
-
-try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-except gflags.DuplicateFlagError:
- pass
-
-def main(argv):
- if FLAGS.plot:
- polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
- elif len(argv) != 7:
- glog.fatal('Expected .h file name and .cc file name')
- else:
- polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7], 'y2012',
- drivetrain.kDrivetrain)
-
-if __name__ == '__main__':
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
diff --git a/y2012/joystick_reader.cc b/y2012/joystick_reader.cc
deleted file mode 100644
index 7a8868f..0000000
--- a/y2012/joystick_reader.cc
+++ /dev/null
@@ -1,170 +0,0 @@
-#include <unistd.h>
-
-#include <cmath>
-#include <cstdio>
-#include <cstring>
-
-#include "aos/actions/actions.h"
-#include "aos/events/shm_event_loop.h"
-#include "aos/init.h"
-#include "aos/logging/logging.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
-#include "frc971/input/driver_station_data.h"
-#include "frc971/input/joystick_input.h"
-#include "y2012/control_loops/accessories/accessories_generated.h"
-
-using ::frc971::input::driver_station::ButtonLocation;
-using ::frc971::input::driver_station::ControlBit;
-using ::frc971::input::driver_station::JoystickAxis;
-
-#define OLD_DS 0
-
-namespace y2012 {
-namespace input {
-namespace joysticks {
-
-const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
-const ButtonLocation kShiftHigh(2, 3), kShiftLow(2, 1);
-const ButtonLocation kQuickTurn(1, 5);
-
-const ButtonLocation kCatch(3, 10);
-
-#if OLD_DS
-const ButtonLocation kFire(3, 11);
-const ButtonLocation kUnload(1, 4);
-const ButtonLocation kReload(1, 2);
-
-const ButtonLocation kRollersOut(3, 12);
-const ButtonLocation kRollersIn(3, 7);
-
-const ButtonLocation kTuck(3, 9);
-const ButtonLocation kIntakePosition(3, 8);
-const ButtonLocation kIntakeOpenPosition(3, 10);
-const ButtonLocation kVerticalTuck(3, 1);
-const JoystickAxis kFlipRobot(3, 3);
-
-const ButtonLocation kLongShot(3, 5);
-const ButtonLocation kCloseShot(3, 2);
-const ButtonLocation kFenderShot(3, 3);
-const ButtonLocation kTrussShot(2, 11);
-const ButtonLocation kHumanPlayerShot(3, 2);
-#else
-const ButtonLocation kFire(3, 9);
-const ButtonLocation kUnload(1, 4);
-const ButtonLocation kReload(1, 2);
-
-const ButtonLocation kRollersOut(3, 8);
-const ButtonLocation kRollersIn(3, 3);
-
-const ButtonLocation kTuck(3, 4);
-const ButtonLocation kIntakePosition(3, 5);
-const ButtonLocation kIntakeOpenPosition(3, 11);
-const ButtonLocation kVerticalTuck(2, 6);
-const JoystickAxis kFlipRobot(3, 3);
-
-const ButtonLocation kLongShot(3, 7);
-const ButtonLocation kCloseShot(3, 6);
-const ButtonLocation kFenderShot(3, 2);
-const ButtonLocation kTrussShot(2, 11);
-const ButtonLocation kHumanPlayerShot(3, 1);
-#endif
-
-const ButtonLocation kUserLeft(2, 7);
-const ButtonLocation kUserRight(2, 10);
-
-const JoystickAxis kAdjustClawGoal(3, 2);
-const JoystickAxis kAdjustClawSeparation(3, 1);
-
-class Reader : public ::frc971::input::JoystickInput {
- public:
- Reader(::aos::EventLoop *event_loop)
- : ::frc971::input::JoystickInput(event_loop),
- drivetrain_goal_sender_(
- event_loop->MakeSender<::frc971::control_loops::drivetrain::Goal>(
- "/drivetrain")),
- accessories_goal_sender_(
- event_loop
- ->MakeSender<::y2012::control_loops::accessories::Message>(
- "/accessories")),
- is_high_gear_(false) {}
-
- void RunIteration(
- const ::frc971::input::driver_station::Data &data) override {
- if (!data.GetControlBit(ControlBit::kAutonomous)) {
- HandleDrivetrain(data);
- HandleTeleop(data);
- }
- }
-
- void HandleDrivetrain(const ::frc971::input::driver_station::Data &data) {
- const double wheel = -data.GetAxis(kSteeringWheel);
- const double throttle = -data.GetAxis(kDriveThrottle);
- if (data.PosEdge(kShiftLow)) {
- is_high_gear_ = false;
- }
- if (data.PosEdge(kShiftHigh)) {
- is_high_gear_ = true;
- }
- auto builder = drivetrain_goal_sender_.MakeBuilder();
- frc971::control_loops::drivetrain::Goal::Builder goal_builder =
- builder.MakeBuilder<frc971::control_loops::drivetrain::Goal>();
- goal_builder.add_wheel(wheel);
- goal_builder.add_throttle(throttle);
- goal_builder.add_highgear(is_high_gear_);
- goal_builder.add_quickturn(data.IsPressed(kQuickTurn));
-
- if (builder.Send(goal_builder.Finish()) !=
- aos::RawSender::Error::kOk) {
- AOS_LOG(WARNING, "sending stick values failed\n");
- }
- }
-
- void HandleTeleop(const ::frc971::input::driver_station::Data &data) {
- auto builder = accessories_goal_sender_.MakeBuilder();
- flatbuffers::Offset<flatbuffers::Vector<uint8_t>> solenoids_offset =
- builder.fbb()->CreateVector<uint8_t>({data.IsPressed(kLongShot),
- data.IsPressed(kCloseShot),
- data.IsPressed(kFenderShot)});
-
- flatbuffers::Offset<flatbuffers::Vector<double>> sticks_offset =
- builder.fbb()->CreateVector<double>(
- {data.GetAxis(kAdjustClawGoal),
- data.GetAxis(kAdjustClawSeparation)});
-
- y2012::control_loops::accessories::Message::Builder message_builder =
- builder.MakeBuilder<y2012::control_loops::accessories::Message>();
- message_builder.add_solenoids(solenoids_offset);
- message_builder.add_sticks(sticks_offset);
- if (builder.Send(message_builder.Finish()) !=
- aos::RawSender::Error::kOk) {
- AOS_LOG(WARNING, "sending accessories goal failed\n");
- }
- }
-
- private:
- ::aos::Sender<::frc971::control_loops::drivetrain::Goal>
- drivetrain_goal_sender_;
- ::aos::Sender<::y2012::control_loops::accessories::Message>
- accessories_goal_sender_;
-
- bool is_high_gear_;
-};
-
-} // namespace joysticks
-} // namespace input
-} // namespace y2012
-
-int main(int argc, char **argv) {
- ::aos::InitGoogle(&argc, &argv);
-
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig("config.json");
-
- ::aos::ShmEventLoop event_loop(&config.message());
- ::y2012::input::joysticks::Reader reader(&event_loop);
-
- event_loop.Run();
-
- return 0;
-}
diff --git a/y2012/wpilib_interface.cc b/y2012/wpilib_interface.cc
deleted file mode 100644
index e3234de..0000000
--- a/y2012/wpilib_interface.cc
+++ /dev/null
@@ -1,291 +0,0 @@
-#include <unistd.h>
-
-#include <chrono>
-#include <cinttypes>
-#include <cstdio>
-#include <cstring>
-#include <functional>
-#include <memory>
-#include <mutex>
-#include <thread>
-
-#include "frc971/wpilib/ahal/AnalogInput.h"
-#include "frc971/wpilib/ahal/Compressor.h"
-#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
-#include "frc971/wpilib/ahal/DriverStation.h"
-#include "frc971/wpilib/ahal/Encoder.h"
-#include "frc971/wpilib/ahal/PowerDistributionPanel.h"
-#include "frc971/wpilib/ahal/Relay.h"
-#include "frc971/wpilib/ahal/Talon.h"
-#include "frc971/wpilib/wpilib_robot_base.h"
-#undef ERROR
-
-#include "aos/events/shm_event_loop.h"
-#include "aos/init.h"
-#include "aos/logging/logging.h"
-#include "aos/stl_mutex/stl_mutex.h"
-#include "aos/time/time.h"
-#include "aos/util/log_interval.h"
-#include "aos/util/phased_loop.h"
-#include "aos/util/wrapping_counter.h"
-#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
-#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
-#include "frc971/input/robot_state_generated.h"
-#include "frc971/wpilib/buffered_pcm.h"
-#include "frc971/wpilib/buffered_solenoid.h"
-#include "frc971/wpilib/dma.h"
-#include "frc971/wpilib/dma_edge_counting.h"
-#include "frc971/wpilib/drivetrain_writer.h"
-#include "frc971/wpilib/encoder_and_potentiometer.h"
-#include "frc971/wpilib/interrupt_edge_counting.h"
-#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/logging_generated.h"
-#include "frc971/wpilib/loop_output_handler.h"
-#include "frc971/wpilib/sensor_reader.h"
-#include "y2012/control_loops/accessories/accessories_generated.h"
-#include "y2012/control_loops/control_loops_generated.h"
-
-namespace accessories = ::y2012::control_loops::accessories;
-using namespace frc;
-using std::make_unique;
-
-namespace y2012 {
-namespace wpilib {
-
-double drivetrain_translate(int32_t in) {
- return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*4x*/) * 1 *
- (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
-}
-
-double drivetrain_velocity_translate(double in) {
- return (1.0 / in) / 256.0 /*cpr*/ * 1 *
- (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
-}
-
-class SensorReader : public ::frc971::wpilib::SensorReader {
- public:
- SensorReader(::aos::ShmEventLoop *event_loop)
- : ::frc971::wpilib::SensorReader(event_loop),
- accessories_position_sender_(
- event_loop->MakeSender<::aos::control_loops::Position>(
- "/accessories")),
- drivetrain_position_sender_(
- event_loop
- ->MakeSender<::frc971::control_loops::drivetrain::Position>(
- "/drivetrain")) {}
-
- void RunIteration() {
- {
- auto builder = drivetrain_position_sender_.MakeBuilder();
-
- frc971::control_loops::drivetrain::Position::Builder position_builder =
- builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
- position_builder.add_right_encoder(
- drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
- position_builder.add_left_encoder(
- -drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
- position_builder.add_left_speed(
- drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
- position_builder.add_right_speed(drivetrain_velocity_translate(
- drivetrain_right_encoder_->GetPeriod()));
-
- builder.CheckOk(builder.Send(position_builder.Finish()));
- }
-
- {
- auto builder = accessories_position_sender_.MakeBuilder();
- builder.CheckOk(builder.Send(
- builder.MakeBuilder<::aos::control_loops::Position>().Finish()));
- }
- }
-
- private:
- ::aos::Sender<::aos::control_loops::Position> accessories_position_sender_;
- ::aos::Sender<::frc971::control_loops::drivetrain::Position>
- drivetrain_position_sender_;
-};
-
-class SolenoidWriter {
- public:
- SolenoidWriter(::aos::ShmEventLoop *event_loop,
- const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
- : event_loop_(event_loop),
- pcm_(pcm),
- drivetrain_fetcher_(
- event_loop_
- ->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
- "/drivetrain")),
- accessories_fetcher_(
- event_loop_
- ->MakeFetcher<::y2012::control_loops::accessories::Message>(
- "/accessories")),
- pneumatics_to_log_sender_(
- event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")) {
- event_loop->set_name("Solenoids");
- event_loop_->SetRuntimeRealtimePriority(27);
-
- event_loop_->AddPhasedLoop([this](int iterations) { Loop(iterations); },
- ::std::chrono::milliseconds(20),
- ::std::chrono::milliseconds(1));
- }
-
- void set_drivetrain_high(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- drivetrain_high_ = ::std::move(s);
- }
-
- void set_drivetrain_low(
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- drivetrain_low_ = ::std::move(s);
- }
-
- void set_s1(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- s1_ = ::std::move(s);
- }
-
- void set_s2(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- s2_ = ::std::move(s);
- }
-
- void set_s3(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
- s3_ = ::std::move(s);
- }
-
- void Loop(const int iterations) {
- if (iterations != 1) {
- AOS_LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
- }
-
- {
- accessories_fetcher_.Fetch();
- if (accessories_fetcher_.get()) {
- s1_->Set(accessories_fetcher_->solenoids()->Get(0));
- s2_->Set(accessories_fetcher_->solenoids()->Get(1));
- s3_->Set(accessories_fetcher_->solenoids()->Get(2));
- }
- }
-
- {
- drivetrain_fetcher_.Fetch();
- if (drivetrain_fetcher_.get()) {
- const bool high = drivetrain_fetcher_->left_high() ||
- drivetrain_fetcher_->right_high();
- drivetrain_high_->Set(high);
- drivetrain_low_->Set(!high);
- }
- }
-
- {
- auto builder = pneumatics_to_log_sender_.MakeBuilder();
-
- ::frc971::wpilib::PneumaticsToLog::Builder to_log_builder =
- builder.MakeBuilder<frc971::wpilib::PneumaticsToLog>();
- pcm_->Flush();
- to_log_builder.add_read_solenoids(pcm_->GetAll());
- builder.CheckOk(builder.Send(to_log_builder.Finish()));
- }
- }
-
- private:
- ::aos::EventLoop *event_loop_;
-
- const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
-
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_high_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_low_;
- ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s1_, s2_, s3_;
-
- ::std::unique_ptr<Compressor> compressor_;
-
- ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
- drivetrain_fetcher_;
- ::aos::Fetcher<::y2012::control_loops::accessories::Message>
- accessories_fetcher_;
-
- aos::Sender<::frc971::wpilib::PneumaticsToLog> pneumatics_to_log_sender_;
-};
-
-class AccessoriesWriter : public ::frc971::wpilib::LoopOutputHandler<
- control_loops::accessories::Message> {
- public:
- AccessoriesWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<
- control_loops::accessories::Message>(event_loop, "/accessories") {}
-
- void set_talon1(::std::unique_ptr<Talon> t) { talon1_ = ::std::move(t); }
-
- void set_talon2(::std::unique_ptr<Talon> t) { talon2_ = ::std::move(t); }
-
- private:
- virtual void Write(
- const control_loops::accessories::Message &output) override {
- talon1_->SetSpeed(output.sticks()->Get(0));
- talon2_->SetSpeed(output.sticks()->Get(1));
- }
-
- virtual void Stop() override {
- AOS_LOG(WARNING, "shooter output too old\n");
- talon1_->SetDisabled();
- talon2_->SetDisabled();
- }
-
- ::std::unique_ptr<Talon> talon1_, talon2_;
-};
-
-class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
- public:
- ::std::unique_ptr<Encoder> make_encoder(int index) {
- return make_unique<Encoder>(10 + index * 2, 11 + index * 2, false,
- Encoder::k4X);
- }
-
- void Run() override {
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig("config.json");
-
- // Thread 1.
- ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
- ::frc971::wpilib::JoystickSender joystick_sender(
- &joystick_sender_event_loop);
- AddLoop(&joystick_sender_event_loop);
-
- // Thread 2.
- ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
- SensorReader sensor_reader(&sensor_reader_event_loop);
- sensor_reader.set_drivetrain_left_encoder(make_encoder(0));
- sensor_reader.set_drivetrain_right_encoder(make_encoder(1));
- AddLoop(&sensor_reader_event_loop);
-
- // Thread 3.
- ::aos::ShmEventLoop output_event_loop(&config.message());
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&output_event_loop);
- drivetrain_writer.set_left_controller0(
- ::std::unique_ptr<Talon>(new Talon(3)), true);
- drivetrain_writer.set_right_controller0(
- ::std::unique_ptr<Talon>(new Talon(4)), false);
-
- ::y2012::wpilib::AccessoriesWriter accessories_writer(&output_event_loop);
- accessories_writer.set_talon1(::std::unique_ptr<Talon>(new Talon(5)));
- accessories_writer.set_talon2(::std::unique_ptr<Talon>(new Talon(6)));
- AddLoop(&output_event_loop);
-
- // Thread 4.
- ::aos::ShmEventLoop solenoid_writer_event_loop(&config.message());
- ::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
- new ::frc971::wpilib::BufferedPcm());
- SolenoidWriter solenoid_writer(&solenoid_writer_event_loop, pcm);
- solenoid_writer.set_drivetrain_high(pcm->MakeSolenoid(0));
- solenoid_writer.set_drivetrain_low(pcm->MakeSolenoid(2));
- solenoid_writer.set_s1(pcm->MakeSolenoid(1));
- solenoid_writer.set_s2(pcm->MakeSolenoid(3));
- solenoid_writer.set_s3(pcm->MakeSolenoid(4));
- AddLoop(&solenoid_writer_event_loop);
-
- RunLoops();
- }
-};
-
-} // namespace wpilib
-} // namespace y2012
-
-AOS_ROBOT_CLASS(::y2012::wpilib::WPILibRobot);
diff --git a/y2018/control_loops/python/BUILD b/y2018/control_loops/python/BUILD
index 3c75b97..c74396c 100644
--- a/y2018/control_loops/python/BUILD
+++ b/y2018/control_loops/python/BUILD
@@ -172,8 +172,6 @@
"graph_generate.py",
],
legacy_create_init = False,
- python_version = "PY3",
- srcs_version = "PY3",
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
":python_init",
@@ -190,11 +188,10 @@
"graph_generate.py",
],
legacy_create_init = False,
- python_version = "PY2",
- srcs_version = "PY2",
target_compatible_with = ["@platforms//os:linux"],
deps = [
":python_init",
+ "@python_repo//:numpy",
],
)
diff --git a/y2020/control_loops/drivetrain/localizer_plotter.ts b/y2020/control_loops/drivetrain/localizer_plotter.ts
index e6729dd..86465a5 100644
--- a/y2020/control_loops/drivetrain/localizer_plotter.ts
+++ b/y2020/control_loops/drivetrain/localizer_plotter.ts
@@ -12,8 +12,6 @@
import Schema = configuration.reflection.Schema;
const TIME = AosPlotter.TIME;
-const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
-const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
export function plotLocalizer(conn: Connection, element: Element) : void {
@@ -27,11 +25,7 @@
const superstructureStatus = aosPlotter.addMessageSource(
'/superstructure', 'y2020.control_loops.superstructure.Status');
- var currentTop = 0;
-
- const imageAcceptedPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const imageAcceptedPlot = aosPlotter.addPlot(element);
imageAcceptedPlot.plot.getAxisLabels().setTitle('Image Acceptance');
imageAcceptedPlot.plot.getAxisLabels().setXLabel(TIME);
imageAcceptedPlot.plot.getAxisLabels().setYLabel('[bool]');
@@ -41,9 +35,7 @@
.setColor(RED)
.setDrawLine(false);
- const impliedXPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const impliedXPlot = aosPlotter.addPlot(element);
impliedXPlot.plot.getAxisLabels().setTitle('Implied Robot X');
impliedXPlot.plot.getAxisLabels().setXLabel(TIME);
impliedXPlot.plot.getAxisLabels().setYLabel('[m]');
@@ -58,9 +50,7 @@
.setColor(GREEN)
.setLabel('Localizer X');
- const impliedYPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const impliedYPlot = aosPlotter.addPlot(element);
impliedYPlot.plot.getAxisLabels().setTitle('Implied Robot Y');
impliedYPlot.plot.getAxisLabels().setXLabel(TIME);
impliedYPlot.plot.getAxisLabels().setYLabel('[m]');
@@ -75,9 +65,7 @@
.setColor(GREEN)
.setLabel('Localizer Y');
- const impliedHeadingPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const impliedHeadingPlot = aosPlotter.addPlot(element);
impliedHeadingPlot.plot.getAxisLabels().setTitle('Implied Robot Theta');
impliedHeadingPlot.plot.getAxisLabels().setXLabel(TIME);
impliedHeadingPlot.plot.getAxisLabels().setYLabel('[rad]');
@@ -89,9 +77,7 @@
.setColor(GREEN)
.setLabel('Localizer Theta');
- const impliedTurretGoalPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const impliedTurretGoalPlot = aosPlotter.addPlot(element);
impliedTurretGoalPlot.plot.getAxisLabels().setTitle('Implied Turret Goal');
impliedTurretGoalPlot.plot.getAxisLabels().setXLabel(TIME);
impliedTurretGoalPlot.plot.getAxisLabels().setYLabel('[rad]');
@@ -102,9 +88,7 @@
impliedTurretGoalPlot.addMessageLine(superstructureStatus, ['aimer', 'turret_position'])
.setColor(GREEN);
- const imageTimingPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const imageTimingPlot = aosPlotter.addPlot(element);
imageTimingPlot.plot.getAxisLabels().setTitle('Timing Plot');
imageTimingPlot.plot.getAxisLabels().setXLabel(TIME);
imageTimingPlot.plot.getAxisLabels().setYLabel('[ns]');
diff --git a/y2020/control_loops/superstructure/accelerator_plotter.ts b/y2020/control_loops/superstructure/accelerator_plotter.ts
index b625a1c..e1bc5bc 100644
--- a/y2020/control_loops/superstructure/accelerator_plotter.ts
+++ b/y2020/control_loops/superstructure/accelerator_plotter.ts
@@ -21,8 +21,7 @@
// Robot Enabled/Disabled and Mode
const velocityPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- currentTop += DEFAULT_HEIGHT / 2;
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
velocityPlot.plot.getAxisLabels().setTitle('Velocity');
velocityPlot.plot.getAxisLabels().setXLabel(TIME);
velocityPlot.plot.getAxisLabels().setYLabel('rad/s');
@@ -38,8 +37,7 @@
velocityPlot.addMessageLine(status, ['shooter', 'accelerator_right', 'dt_angular_velocity']).setColor(BLUE).setPointSize(0.0);
const voltagePlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- currentTop += DEFAULT_HEIGHT / 2;
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
voltagePlot.plot.getAxisLabels().setTitle('Voltage');
voltagePlot.plot.getAxisLabels().setXLabel(TIME);
voltagePlot.plot.getAxisLabels().setYLabel('Volts');
@@ -53,8 +51,7 @@
const currentPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- currentTop += DEFAULT_HEIGHT / 2;
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
currentPlot.plot.getAxisLabels().setTitle('Current');
currentPlot.plot.getAxisLabels().setXLabel(TIME);
currentPlot.plot.getAxisLabels().setYLabel('Amps');
diff --git a/y2020/control_loops/superstructure/finisher_plotter.ts b/y2020/control_loops/superstructure/finisher_plotter.ts
index c9420ae..474c8a4 100644
--- a/y2020/control_loops/superstructure/finisher_plotter.ts
+++ b/y2020/control_loops/superstructure/finisher_plotter.ts
@@ -17,12 +17,9 @@
const pdpValues = aosPlotter.addMessageSource('/roborio/aos', 'frc971.PDPValues');
const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
- var currentTop = 0;
-
// Robot Enabled/Disabled and Mode
const velocityPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- currentTop += DEFAULT_HEIGHT / 2;
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
velocityPlot.plot.getAxisLabels().setTitle('Velocity');
velocityPlot.plot.getAxisLabels().setXLabel(TIME);
velocityPlot.plot.getAxisLabels().setYLabel('rad/s');
@@ -35,8 +32,7 @@
velocityPlot.addMessageLine(status, ['shooter', 'finisher', 'dt_angular_velocity']).setColor(PINK).setPointSize(0.0);
const ballsShotPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- currentTop += DEFAULT_HEIGHT / 2;
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
ballsShotPlot.plot.getAxisLabels().setTitle('Balls Shot');
ballsShotPlot.plot.getAxisLabels().setXLabel(TIME);
ballsShotPlot.plot.getAxisLabels().setYLabel('Balls');
@@ -45,8 +41,7 @@
const voltagePlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- currentTop += DEFAULT_HEIGHT / 2;
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
voltagePlot.plot.getAxisLabels().setTitle('Voltage');
voltagePlot.plot.getAxisLabels().setXLabel(TIME);
voltagePlot.plot.getAxisLabels().setYLabel('Volts');
@@ -58,8 +53,7 @@
const currentPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
- currentTop += DEFAULT_HEIGHT / 2;
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
currentPlot.plot.getAxisLabels().setTitle('Current');
currentPlot.plot.getAxisLabels().setXLabel(TIME);
currentPlot.plot.getAxisLabels().setYLabel('Amps');
diff --git a/y2020/control_loops/superstructure/hood_plotter.ts b/y2020/control_loops/superstructure/hood_plotter.ts
index 6c8025d..55b1ba8 100644
--- a/y2020/control_loops/superstructure/hood_plotter.ts
+++ b/y2020/control_loops/superstructure/hood_plotter.ts
@@ -20,7 +20,7 @@
// Robot Enabled/Disabled and Mode
const positionPlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
currentTop += DEFAULT_HEIGHT / 2;
positionPlot.plot.getAxisLabels().setTitle('Position');
positionPlot.plot.getAxisLabels().setXLabel(TIME);
@@ -35,7 +35,7 @@
positionPlot.addMessageLine(status, ['hood', 'estimator_state', 'position']).setColor(CYAN).setPointSize(0.0);
const voltagePlot =
- aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
currentTop += DEFAULT_HEIGHT / 2;
voltagePlot.plot.getAxisLabels().setTitle('Voltage');
voltagePlot.plot.getAxisLabels().setXLabel(TIME);
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index 1fb943f..a25ed53 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -42,7 +42,7 @@
const double a = 1;
const double b = -bemf_voltage;
const double c_positive = -70.0 * 12.0 * resistance_;
- const double c_negative = -40.0 * 12.0 * resistance_;
+ const double c_negative = -25.0 * 12.0 * resistance_;
// Root is always positive.
const double root_positive = std::sqrt(b * b - 4.0 * a * c_positive);
@@ -52,8 +52,8 @@
// Limit to the battery voltage and the current limit voltage.
mutable_U(0, 0) = std::clamp(U(0, 0), lower_limit, upper_limit);
- if (R(0) > 50.0) {
- mutable_U(0, 0) = std::clamp(U(0, 0), -0.8, 12.0);
+ if (R(1) > 50.0) {
+ mutable_U(0, 0) = std::clamp(U(0, 0), 1.0, 12.0);
} else {
mutable_U(0, 0) = std::clamp(U(0, 0), 0.0, 12.0);
}
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index bd1e74c..f70f107 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -284,6 +284,12 @@
output_struct.feeder_voltage = -12.0;
}
+ if (unsafe_goal->has_feed_voltage_override()) {
+ output_struct.feeder_voltage = unsafe_goal->feed_voltage_override();
+ output_struct.washing_machine_spinner_voltage = -5.0;
+ preloading_timeout_ = position_timestamp;
+ }
+
if (unsafe_goal->shooting()) {
if ((shooter_.ready() ||
(!has_turret_ && shooter_.accelerator_ready())) &&
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
index 990234a..f2b9096 100644
--- a/y2020/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -55,6 +55,9 @@
// Positive is deploying climber and to climb; cannot run in reverse
climber_voltage:float (id: 10);
+
+ // Feed voltage override.
+ feed_voltage_override:float (id: 13);
}
root_type Goal;
diff --git a/y2020/control_loops/superstructure/turret_plotter.ts b/y2020/control_loops/superstructure/turret_plotter.ts
index 948ca97..34279ba 100644
--- a/y2020/control_loops/superstructure/turret_plotter.ts
+++ b/y2020/control_loops/superstructure/turret_plotter.ts
@@ -12,8 +12,6 @@
import Schema = configuration.reflection.Schema;
const TIME = AosPlotter.TIME;
-const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
-const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
class DerivativeMessageHandler extends MessageHandler {
// Calculated magnitude of the measured acceleration from the IMU.
@@ -71,11 +69,7 @@
const localizerDebug =
aosPlotter.addMessageSource('/drivetrain', 'y2020.control_loops.drivetrain.LocalizerDebug');
- var currentTop = 0;
-
- const turretPosPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const turretPosPlot = aosPlotter.addPlot(element);
turretPosPlot.plot.getAxisLabels().setTitle('Turret Position');
turretPosPlot.plot.getAxisLabels().setXLabel(TIME);
turretPosPlot.plot.getAxisLabels().setYLabel('rad');
@@ -93,9 +87,7 @@
.setColor(BLUE)
.setDrawLine(false);
- const turretVelPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const turretVelPlot = aosPlotter.addPlot(element);
turretVelPlot.plot.getAxisLabels().setTitle('Turret Velocity');
turretVelPlot.plot.getAxisLabels().setXLabel(TIME);
turretVelPlot.plot.getAxisLabels().setYLabel('rad / sec');
@@ -110,9 +102,7 @@
.setColor(BLUE)
.setDrawLine(false);
- const turretAccelPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const turretAccelPlot = aosPlotter.addPlot(element);
turretAccelPlot.plot.getAxisLabels().setTitle('Turret Acceleration');
turretAccelPlot.plot.getAxisLabels().setXLabel(TIME);
turretAccelPlot.plot.getAxisLabels().setYLabel('rad / sec / sec');
@@ -121,9 +111,7 @@
.setColor(RED)
.setPointSize(0.0);
- const turretVoltagePlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const turretVoltagePlot = aosPlotter.addPlot(element);
turretVoltagePlot.plot.getAxisLabels().setTitle('Turret Voltage');
turretVoltagePlot.plot.getAxisLabels().setXLabel(TIME);
turretVoltagePlot.plot.getAxisLabels().setYLabel('V');
@@ -141,9 +129,7 @@
.setColor(RED)
.setPointSize(0.0);
- const currentPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const currentPlot = aosPlotter.addPlot(element);
currentPlot.plot.getAxisLabels().setTitle('Current');
currentPlot.plot.getAxisLabels().setXLabel(TIME);
currentPlot.plot.getAxisLabels().setYLabel('Amps');
@@ -154,9 +140,7 @@
.setPointSize(0.0);
- const targetDistancePlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const targetDistancePlot = aosPlotter.addPlot(element);
targetDistancePlot.plot.getAxisLabels().setTitle('Target distance');
targetDistancePlot.plot.getAxisLabels().setXLabel(TIME);
targetDistancePlot.plot.getAxisLabels().setYLabel('m');
@@ -165,9 +149,7 @@
.setColor(RED)
.setPointSize(0.0);
- const targetChoicePlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const targetChoicePlot = aosPlotter.addPlot(element);
targetChoicePlot.plot.getAxisLabels().setTitle('Target choice');
targetChoicePlot.plot.getAxisLabels().setXLabel(TIME);
targetChoicePlot.plot.getAxisLabels().setYLabel('[bool]');
@@ -177,9 +159,7 @@
.setColor(RED)
.setPointSize(0.0);
- const imageAcceptedPlot = aosPlotter.addPlot(
- element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- currentTop += DEFAULT_HEIGHT;
+ const imageAcceptedPlot = aosPlotter.addPlot(element);
imageAcceptedPlot.plot.getAxisLabels().setTitle('Image Acceptance');
imageAcceptedPlot.plot.getAxisLabels().setXLabel(TIME);
imageAcceptedPlot.plot.getAxisLabels().setYLabel('[bool]');
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index f1237e1..b610b64 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -295,6 +295,9 @@
superstructure_goal_builder.add_shooter(shooter_offset);
superstructure_goal_builder.add_shooting(data.IsPressed(kFeed) ||
data.IsPressed(kFeedDriver));
+ if (data.IsPressed(kSpit)) {
+ superstructure_goal_builder.add_feed_voltage_override(-7.0);
+ }
superstructure_goal_builder.add_climber_voltage(climber_speed);
superstructure_goal_builder.add_turret_tracking(turret_tracking);
diff --git a/y2020/vision/sift/BUILD b/y2020/vision/sift/BUILD
index 98c6ec6..93e2858 100644
--- a/y2020/vision/sift/BUILD
+++ b/y2020/vision/sift/BUILD
@@ -25,8 +25,6 @@
"@amd64_debian_sysroot//:sysroot_files",
],
main = "fast_gaussian_runner.py",
- python_version = "PY3",
- srcs_version = "PY2AND3",
target_compatible_with = ["@platforms//os:linux"],
toolchains = [
"@bazel_tools//tools/cpp:current_cc_toolchain",
@@ -220,8 +218,6 @@
py_binary(
name = "demo_sift_training",
srcs = ["demo_sift_training.py"],
- python_version = "PY3",
- srcs_version = "PY2AND3",
target_compatible_with = ["@platforms//os:linux"],
deps = [
":sift_fbs_python",
diff --git a/y2020/vision/tools/python_code/BUILD b/y2020/vision/tools/python_code/BUILD
index fc7322d..7579a75 100644
--- a/y2020/vision/tools/python_code/BUILD
+++ b/y2020/vision/tools/python_code/BUILD
@@ -4,12 +4,10 @@
py_library(
name = "train_and_match",
srcs = ["train_and_match.py"],
- data = [
- "@python_repo//:scipy",
- ],
deps = [
"//external:python-glog",
"@opencv_contrib_nonfree_amd64//:python_opencv",
+ "@python_repo//:scipy",
],
)
@@ -18,13 +16,11 @@
srcs = [
"define_training_data.py",
],
- data = [
- "@python_repo//:scipy",
- ],
deps = [
":train_and_match",
"//external:python-glog",
"@opencv_contrib_nonfree_amd64//:python_opencv",
+ "@python_repo//:scipy",
],
)
@@ -61,7 +57,6 @@
"test_images/*.png",
]),
main = "target_definition.py",
- python_version = "PY3",
target_compatible_with = ["@platforms//os:linux"],
deps = [
":target_definition",
@@ -80,7 +75,6 @@
data = glob(["calib_files/*.json"]) + glob([
"test_images/*.png",
]),
- python_version = "PY3",
target_compatible_with = ["@platforms//os:linux"],
deps = [
":camera_definition",
@@ -100,7 +94,6 @@
data = glob(["calib_files/*.json"]) + glob([
"test_images/*.png",
]),
- python_version = "PY3",
target_compatible_with = ["@platforms//os:linux"],
deps = [
":camera_definition",
@@ -156,8 +149,6 @@
"test_images/*.png",
]),
main = "load_sift_training.py",
- python_version = "PY3",
- srcs_version = "PY2AND3",
target_compatible_with = ["@platforms//os:linux"],
deps = [
":load_sift_training",
diff --git a/y2020/y2020_logger.json b/y2020/y2020_logger.json
index 0d97a7d..eba9996 100644
--- a/y2020/y2020_logger.json
+++ b/y2020/y2020_logger.json
@@ -141,6 +141,7 @@
"type": "aos.message_bridge.ClientStatistics",
"source_node": "logger",
"frequency": 10,
+ "max_size": 2000,
"num_senders": 2
},
{
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 8bab794..a7f17b0 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -55,7 +55,7 @@
"type": "aos.message_bridge.ClientStatistics",
"source_node": "roborio",
"frequency": 15,
- "max_size": 736,
+ "max_size": 2000,
"num_senders": 2
},
{
diff --git a/y2021_bot3/control_loops/superstructure/BUILD b/y2021_bot3/control_loops/superstructure/BUILD
index 77cdf45..e4dd6a2 100644
--- a/y2021_bot3/control_loops/superstructure/BUILD
+++ b/y2021_bot3/control_loops/superstructure/BUILD
@@ -1,6 +1,7 @@
package(default_visibility = ["//visibility:public"])
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@npm_bazel_typescript//:defs.bzl", "ts_library")
flatbuffer_cc_library(
name = "superstructure_goal_fbs",
@@ -101,4 +102,15 @@
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
],
+)
+
+ts_library(
+ name = "superstructure_plotter",
+ srcs = ["superstructure_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ ],
)
\ No newline at end of file
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
index 7ef6bf3..e30fa1d 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -1,13 +1,16 @@
#include <chrono>
#include <memory>
-#include "aos/events/logging/log_writer.h"
#include "frc971/control_loops/capped_test_plant.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2021_bot3/control_loops/superstructure/superstructure.h"
+#include "aos/events/logging/log_writer.h"
+
+DEFINE_string(output_folder, "",
+ "If set, logs all channels to the provided logfile.");
namespace y2021_bot3 {
namespace control_loops {
@@ -40,6 +43,13 @@
phased_loop_handle_ = test_event_loop_->AddPhasedLoop(
[this](int) { SendPositionMessage(); }, dt());
+
+ if (!FLAGS_output_folder.empty()) {
+ unlink(FLAGS_output_folder.c_str());
+ logger_event_loop_ = MakeEventLoop("logger", roborio_);
+ logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
+ logger_->StartLoggingLocalNamerOnRun(FLAGS_output_folder);
+ }
}
void VerifyResults(double intake_voltage, double outtake_voltage,
@@ -65,6 +75,9 @@
builder.CheckOk(builder.Send(position_builder.Finish()));
}
+ // Because the third robot is single node, the roborio node is nullptr
+ const aos::Node *const roborio_ = nullptr;
+
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
::y2021_bot3::control_loops::superstructure::Superstructure superstructure_;
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
@@ -75,6 +88,8 @@
::aos::Fetcher<Output> superstructure_output_fetcher_;
::aos::Fetcher<Position> superstructure_position_fetcher_;
::aos::Sender<Position> superstructure_position_sender_;
+ std::unique_ptr<aos::EventLoop> logger_event_loop_;
+ std::unique_ptr<aos::logger::Logger> logger_;
};
// Tests running the intake and outtake separately
@@ -167,6 +182,25 @@
VerifyResults(0.0, 0.0, 0.0, 6.0, 5.0, 4.0);
}
+TEST_F(SuperstructureTest, PlotterTest) {
+ double speed = 10.0;
+ test_event_loop_->AddPhasedLoop(
+ [&](int) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(speed);
+ goal_builder.add_outtake_speed(speed);
+ goal_builder.add_climber_speed(speed);
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ speed += .001;
+ if (speed >= 12) {
+ speed = -12;
+ }
+ },
+ frc971::controls::kLoopFrequency);
+ RunFor(std::chrono::seconds(10));
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_plotter.ts b/y2021_bot3/control_loops/superstructure/superstructure_plotter.ts
new file mode 100644
index 0000000..f453d64
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure_plotter.ts
@@ -0,0 +1,41 @@
+// Provides a plot for debugging robot state-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
+
+export function plotSuperstructure(conn: Connection, element: Element) : void {
+ const aosPlotter = new AosPlotter(conn);
+ const goal = aosPlotter.addMessageSource('/superstructure', 'y2021_bot3.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource('/superstructure', 'y2021_bot3.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource('/superstructure', 'y2021_bot3.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource('/superstructure', 'y2021_bot3.control_loops.superstructure.Position');
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+ const intakePlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ intakePlot.plot.getAxisLabels().setTitle('Intake');
+ intakePlot.plot.getAxisLabels().setXLabel(TIME);
+ intakePlot.plot.getAxisLabels().setYLabel('Volts');
+ intakePlot.plot.setDefaultYRange([-20.0, 20.0]);
+
+ intakePlot.addMessageLine(output, ['intake_volts']).setColor(BLUE);
+ intakePlot.addMessageLine(goal, ['intake_speed']).setColor(GREEN);
+ intakePlot.addMessageLine(status, ['intake_speed']).setColor(RED);
+
+ const outtakePlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ outtakePlot.plot.getAxisLabels().setTitle('Outtake');
+ outtakePlot.plot.getAxisLabels().setXLabel(TIME);
+ outtakePlot.plot.getAxisLabels().setYLabel('Volts');
+ outtakePlot.plot.setDefaultYRange([-20.0, 20.0]);
+
+ outtakePlot.addMessageLine(output, ['outtake_volts']).setColor(BLUE);
+ outtakePlot.addMessageLine(goal, ['outtake_speed']).setColor(GREEN);
+ outtakePlot.addMessageLine(status, ['outtake_speed']).setColor(RED);
+}