Merge "Scouting app: Add "actions" to database for 2023 scouting changes"
diff --git a/WORKSPACE b/WORKSPACE
index c926b0e..aa92b7e 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -549,6 +549,18 @@
actual = "@com_google_googletest//:gtest_main",
)
+http_archive(
+ name = "april_tag_test_image",
+ build_file_content = """
+filegroup(
+ name = "april_tag_test_image",
+ srcs = ["test.bfbs", "expected.jpeg", "expected.png"],
+ visibility = ["//visibility:public"],
+)""",
+ sha256 = "5312c79b19e9883b3cebd9d65b4438a2bf05b41da0bcd8c35e19d22c3b2e1859",
+ urls = ["https://www.frc971.org/Build-Dependencies/test_image_frc971.vision.CameraImage_2023.01.28.tar.gz"],
+)
+
# Recompressed from libusb-1.0.21.7z.
http_file(
name = "libusb_1_0_windows",
@@ -1228,7 +1240,7 @@
)
# This one is tricky to get an archive because it has recursive submodules. These semi-automated steps do work though:
-# git clone -b version1.9 --recurse-submodules --depth=1 https://github.com/aws/aws-sdk-cpp
+# git clone -b 1.10.34 --recurse-submodules --depth=1 https://github.com/aws/aws-sdk-cpp
# cd aws-sdk-cpp
# echo bsdtar -a -cf aws_sdk-version.tar.gz --ignore-zeros @\<\(git archive HEAD\) $(git submodule foreach --recursive --quiet 'echo @\<\(cd $displaypath \&\& git archive HEAD --prefix=$displaypath/\)')
# Now run the command that printed, and the output will be at aws_sdk-version.tar.gz.
@@ -1237,8 +1249,8 @@
build_file = "//debian:aws_sdk.BUILD",
patch_args = ["-p1"],
patches = ["//debian:aws_sdk.patch"],
- sha256 = "1a2668722e5b5b2608a6ab21c876c1d98b5fd8c7d613129ced9561f5520817dc",
- url = "https://www.frc971.org/Build-Dependencies/aws_sdk-19.0.0-RC1.tar.gz",
+ sha256 = "de6570d10c246189fd8c02100f7f0d9af8499a3ef94a131eeb85619f3bd6c604",
+ url = "https://www.frc971.org/Build-Dependencies/aws_sdk-1.10.34.tar.gz",
)
# Source code of LZ4 (files under lib/) are under BSD 2-Clause.
@@ -1303,9 +1315,9 @@
http_archive(
name = "com_github_foxglove_ws-protocol",
- build_file = "//third_party/foxglove_ws_protocol:foxglove_ws_protocol.BUILD",
+ build_file = "//third_party/foxglove/ws_protocol:foxglove_ws_protocol.BUILD",
patch_args = ["-p1"],
- patches = ["//third_party/foxglove_ws_protocol:foxglove_ws_protocol.patch"],
+ patches = ["//third_party/foxglove/ws_protocol:foxglove_ws_protocol.patch"],
sha256 = "3256f09a67419f6556778c443d332f1a4bf53ba0e7a464179bf838abffa366ab",
strip_prefix = "ws-protocol-releases-typescript-ws-protocol-examples-v0.0.6",
url = "https://github.com/foxglove/ws-protocol/archive/refs/tags/releases/typescript/ws-protocol-examples/v0.0.6.tar.gz",
@@ -1325,3 +1337,11 @@
strip_prefix = "asio-1.24.0",
url = "https://downloads.sourceforge.net/project/asio/asio/1.24.0%2520%2528Stable%2529/asio-1.24.0.tar.bz2",
)
+
+http_archive(
+ name = "com_github_foxglove_schemas",
+ build_file = "//third_party/foxglove/schemas:schemas.BUILD",
+ sha256 = "c0d08365eb8fba0af7773b5f0095fb53fb53f020bde46edaa308af5bb939fc15",
+ strip_prefix = "schemas-7a3e077b88142ac46bb4e2616f83dc029b45352e/schemas/flatbuffer",
+ url = "https://github.com/foxglove/schemas/archive/7a3e077b88142ac46bb4e2616f83dc029b45352e.tar.gz",
+)
diff --git a/aos/BUILD b/aos/BUILD
index 9768753..b8468bd 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -157,7 +157,10 @@
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
"//aos/time",
+ "//aos/util:top",
"@com_github_google_glog//:glog",
],
)
diff --git a/aos/configuration.cc b/aos/configuration.cc
index ad6fb54..1d5e60a 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -1618,5 +1618,32 @@
return RecursiveCopyFlatBuffer(found_schema);
}
+aos::FlatbufferDetachedBuffer<Configuration> AddChannelToConfiguration(
+ const Configuration *config, std::string_view name,
+ aos::FlatbufferVector<reflection::Schema> schema, const aos::Node *node,
+ ChannelT overrides) {
+ overrides.name = name;
+ CHECK(schema.message().has_root_table());
+ overrides.type = schema.message().root_table()->name()->string_view();
+ if (node != nullptr) {
+ CHECK(node->has_name());
+ overrides.source_node = node->name()->string_view();
+ }
+ flatbuffers::FlatBufferBuilder fbb;
+ // Don't populate fields from overrides that the user doesn't explicitly
+ // override.
+ fbb.ForceDefaults(false);
+ const flatbuffers::Offset<Channel> channel_offset =
+ Channel::Pack(fbb, &overrides);
+ const flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Channel>>>
+ channels_offset = fbb.CreateVector({channel_offset});
+ Configuration::Builder config_builder(fbb);
+ config_builder.add_channels(channels_offset);
+ fbb.Finish(config_builder.Finish());
+ FlatbufferDetachedBuffer<Configuration> new_channel_config = fbb.Release();
+ new_channel_config = MergeConfiguration(new_channel_config, {schema});
+ return MergeWithConfig(config, new_channel_config);
+}
+
} // namespace configuration
} // namespace aos
diff --git a/aos/configuration.h b/aos/configuration.h
index 4d84b23..0e5ef74 100644
--- a/aos/configuration.h
+++ b/aos/configuration.h
@@ -227,6 +227,15 @@
GetSchemaDetachedBuffer(const Configuration *config,
std::string_view schema_type);
+// Adds the specified channel to the config and returns the new, merged, config.
+// The channel name is derived from the specified name, the type and schema from
+// the provided schema, the source node from the specified node, and all other
+// fields (e.g., frequency) will be derived from the overrides parameter.
+aos::FlatbufferDetachedBuffer<Configuration> AddChannelToConfiguration(
+ const Configuration *config, std::string_view name,
+ aos::FlatbufferVector<reflection::Schema> schema,
+ const aos::Node *source_node = nullptr, ChannelT overrides = {});
+
} // namespace configuration
// Compare and equality operators for Channel. Note: these only check the name
diff --git a/aos/configuration_test.cc b/aos/configuration_test.cc
index fc45d88..823f43c 100644
--- a/aos/configuration_test.cc
+++ b/aos/configuration_test.cc
@@ -1049,6 +1049,60 @@
std::nullopt);
}
+// Tests that we can use a utility to add individual channels to a single-node
+// config.
+TEST_F(ConfigurationTest, AddChannelToConfigSingleNode) {
+ FlatbufferDetachedBuffer<Configuration> base_config =
+ ReadConfig(ArtifactPath("aos/testdata/config1.json"));
+
+ FlatbufferVector<reflection::Schema> schema =
+ FileToFlatbuffer<reflection::Schema>(
+ ArtifactPath("aos/events/ping.bfbs"));
+
+ FlatbufferDetachedBuffer<Configuration> new_config =
+ AddChannelToConfiguration(&base_config.message(), "/new", schema);
+
+ ASSERT_EQ(new_config.message().channels()->size(),
+ base_config.message().channels()->size() + 1);
+
+ const Channel *channel =
+ GetChannel(new_config, "/new", "aos.examples.Ping", "", nullptr);
+ ASSERT_TRUE(channel != nullptr);
+ ASSERT_TRUE(channel->has_schema());
+ // Check that we don't populate channel settings that we don't override the
+ // defaults of.
+ ASSERT_FALSE(channel->has_frequency());
+}
+
+// Tests that we can use a utility to add individual channels to a multi-node
+// config.
+TEST_F(ConfigurationTest, AddChannelToConfigMultiNode) {
+ FlatbufferDetachedBuffer<Configuration> base_config =
+ ReadConfig(ArtifactPath("aos/testdata/good_multinode.json"));
+
+ FlatbufferVector<reflection::Schema> schema =
+ FileToFlatbuffer<reflection::Schema>(
+ ArtifactPath("aos/events/ping.bfbs"));
+
+ aos::ChannelT channel_overrides;
+ channel_overrides.frequency = 971;
+ FlatbufferDetachedBuffer<Configuration> new_config =
+ AddChannelToConfiguration(&base_config.message(), "/new", schema,
+ GetNode(&base_config.message(), "pi1"),
+ channel_overrides);
+
+ ASSERT_EQ(new_config.message().channels()->size(),
+ base_config.message().channels()->size() + 1);
+
+ const Channel *channel =
+ GetChannel(new_config, "/new", "aos.examples.Ping", "", nullptr);
+ ASSERT_TRUE(channel != nullptr);
+ ASSERT_TRUE(channel->has_schema());
+ ASSERT_TRUE(channel->has_source_node());
+ ASSERT_EQ("pi1", channel->source_node()->string_view());
+ ASSERT_EQ(971, channel->frequency());
+}
+
} // namespace testing
} // namespace configuration
} // namespace aos
diff --git a/aos/containers/priority_queue.h b/aos/containers/priority_queue.h
index b092219..d4d0196 100644
--- a/aos/containers/priority_queue.h
+++ b/aos/containers/priority_queue.h
@@ -3,6 +3,7 @@
#include <array>
#include <iterator>
+#include <optional>
namespace aos {
@@ -60,18 +61,19 @@
// and the queue is full, then data is ignored and end() is returned.
// PushFromBottom starts the search from the bottom of the queue.
// TODO(james): If performance becomes an issue, improve search.
- iterator PushFromBottom(const Data &data) {
+ iterator PushFromBottom(Data data) {
size_t lower_idx = buffer_size;
size_t upper_idx = bottom_;
// Find our spot in the queue:
- while (upper_idx != buffer_size && !cmp_(data, list_[upper_idx].data)) {
+ while (upper_idx != buffer_size &&
+ !cmp_(data, list_[upper_idx].data.value())) {
lower_idx = upper_idx;
upper_idx = list_[upper_idx].upper_idx;
if (upper_idx == buffer_size) {
break;
}
}
- return InsertInList(data, lower_idx, upper_idx);
+ return InsertInList(std::move(data), lower_idx, upper_idx);
}
size_t size() const { return size_; }
@@ -85,10 +87,10 @@
top_ = buffer_size;
}
- Data &top() { return list_[top_].data; }
- const Data &top() const { return list_[top_].data; }
- Data &get(size_t idx) { return list_[idx].data; }
- const Data &get(size_t idx) const { return list_[idx].data; }
+ Data &top() { return list_[top_].data.value(); }
+ const Data &top() const { return list_[top_].data.value(); }
+ Data &get(size_t idx) { return list_[idx].data.value(); }
+ const Data &get(size_t idx) const { return list_[idx].data.value(); }
iterator begin() { return iterator(this, bottom_); }
iterator end() { return iterator(this, buffer_size); }
@@ -101,7 +103,7 @@
struct Datum {
// A list element with data and the indices of the next highest/lowest
// priority elements.
- Data data;
+ std::optional<Data> data;
// Values of buffer_size indicate that we are at the beginning or end of
// the queue.
size_t lower_idx = buffer_size;
@@ -109,7 +111,7 @@
};
// Insert an element above lower_idx and below upper_idx.
- iterator InsertInList(const Data &data, size_t lower_idx, size_t upper_idx) {
+ iterator InsertInList(Data data, size_t lower_idx, size_t upper_idx) {
// For inserting new elements, when we are initially filling the queue we
// will increment upwards in the array; once full, we just evict the
// lowest priority element.
@@ -140,7 +142,7 @@
if (top_ == lower_idx) {
top_ = insertion_idx;
}
- list_[insertion_idx].data = data;
+ list_[insertion_idx].data.emplace(std::move(data));
list_[insertion_idx].upper_idx = upper_idx;
list_[insertion_idx].lower_idx = lower_idx;
++size_;
diff --git a/aos/containers/priority_queue_test.cc b/aos/containers/priority_queue_test.cc
index d26d7c9..64456c4 100644
--- a/aos/containers/priority_queue_test.cc
+++ b/aos/containers/priority_queue_test.cc
@@ -153,16 +153,22 @@
ASSERT_TRUE(reverse_expected.empty());
}
-// Check that operator-> works as expected on the iterator.
+// Check that operator-> works as expected on the iterator, and that we can use
+// a non-copyable, non-assignable object.
struct TestStruct {
+ TestStruct(int a) : a(a) {}
+ TestStruct(const TestStruct &) = delete;
+ TestStruct &operator=(const TestStruct &) = delete;
+ TestStruct(TestStruct &&) = default;
+ TestStruct &operator=(TestStruct &&) = delete;
int a;
friend bool operator<(const TestStruct &lhs, const TestStruct &rhs) {
return lhs.a < rhs.a;
}
};
-TEST(PriorirtyQueueTest, MemberAccess) {
+TEST(PriorityQueueMoveTest, MemberAccess) {
PriorityQueue<TestStruct, 10, ::std::less<TestStruct>> q;
- auto it = q.PushFromBottom({11});
+ auto it = q.PushFromBottom(TestStruct{11});
EXPECT_EQ(11, it->a);
}
diff --git a/aos/dump_rtprio.cc b/aos/dump_rtprio.cc
index 8020857..d36a1c0 100644
--- a/aos/dump_rtprio.cc
+++ b/aos/dump_rtprio.cc
@@ -18,9 +18,14 @@
#include <set>
#include <string>
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/util/top.h"
#include "aos/time/time.h"
#include "glog/logging.h"
+DEFINE_string(config, "aos_config.json", "File path of aos configuration");
+
namespace {
const char *policy_string(uint32_t policy) {
@@ -52,17 +57,7 @@
return str.substr(0, str.size() - 1);
}
-int find_pid_max() {
- int r;
- FILE *pid_max_file = fopen("/proc/sys/kernel/pid_max", "r");
- PCHECK(pid_max_file != nullptr)
- << ": Failed to open /proc/sys/kernel/pid_max";
- CHECK_EQ(1, fscanf(pid_max_file, "%d", &r));
- PCHECK(fclose(pid_max_file) == 0);
- return r;
-}
-
-cpu_set_t find_all_cpus() {
+cpu_set_t FindAllCpus() {
long nproc = sysconf(_SC_NPROCESSORS_CONF);
PCHECK(nproc != -1);
cpu_set_t r;
@@ -253,47 +248,63 @@
} // namespace
-int main() {
- const int pid_max = find_pid_max();
- const cpu_set_t all_cpus = find_all_cpus();
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
- std::multiset<Thread> threads;
+ aos::ShmEventLoop event_loop(&config.message());
+ event_loop.SkipTimingReport();
+ event_loop.SkipAosLog();
+ aos::util::Top top(&event_loop);
+ top.set_track_top_processes(true);
- for (int i = 0; i < pid_max; ++i) {
- bool not_there = false;
+ const cpu_set_t all_cpus = FindAllCpus();
- const cpu_set_t cpu_mask = find_cpu_mask(i, ¬_there);
- const sched_param param = find_sched_param(i, ¬_there);
- const int scheduler = find_scheduler(i, ¬_there);
- const ::std::string exe = find_exe(i, ¬_there);
- const int nice_value = find_nice_value(i, ¬_there);
+ top.set_on_reading_update([&]() {
+ std::multiset<Thread> threads;
- int ppid = 0, sid = 0;
- read_stat(i, &ppid, &sid, ¬_there);
+ for (const std::pair<const pid_t, aos::util::Top::ProcessReadings>
+ &reading : top.readings()) {
+ pid_t tid = reading.first;
+ bool not_there = false;
- int pgrp = 0;
- ::std::string name;
- read_status(i, ppid, &pgrp, &name, ¬_there);
+ const cpu_set_t cpu_mask = find_cpu_mask(tid, ¬_there);
+ const sched_param param = find_sched_param(tid, ¬_there);
+ const int scheduler = find_scheduler(tid, ¬_there);
+ const ::std::string exe = find_exe(tid, ¬_there);
+ const int nice_value = find_nice_value(tid, ¬_there);
- if (not_there) continue;
+ int ppid = 0, sid = 0;
+ read_stat(tid, &ppid, &sid, ¬_there);
- const char *cpu_mask_string =
- CPU_EQUAL(&cpu_mask, &all_cpus) ? "all" : "???";
+ int pgrp = 0;
+ ::std::string name;
+ read_status(tid, ppid, &pgrp, &name, ¬_there);
- threads.emplace(Thread{.policy = static_cast<uint32_t>(scheduler),
- .exe = exe,
- .name = name,
- .cpu_mask = cpu_mask_string,
- .nice_value = nice_value,
- .sched_priority = param.sched_priority,
- .tid = i,
- .pid = pgrp,
- .ppid = ppid,
- .sid = sid});
- }
+ if (not_there) continue;
- printf("exe,name,cpumask,policy,nice,priority,tid,pid,ppid,sid\n");
- for (const auto &t : threads) {
- t.Print();
- }
+ const char *cpu_mask_string =
+ CPU_EQUAL(&cpu_mask, &all_cpus) ? "all" : "???";
+
+ threads.emplace(Thread{.policy = static_cast<uint32_t>(scheduler),
+ .exe = exe,
+ .name = name,
+ .cpu_mask = cpu_mask_string,
+ .nice_value = nice_value,
+ .sched_priority = param.sched_priority,
+ .tid = tid,
+ .pid = pgrp,
+ .ppid = ppid,
+ .sid = sid});
+ }
+
+ printf("exe,name,cpumask,policy,nice,priority,tid,pid,ppid,sid\n");
+ for (const auto &t : threads) {
+ t.Print();
+ }
+ event_loop.Exit();
+ });
+
+ event_loop.Run();
}
diff --git a/aos/events/BUILD b/aos/events/BUILD
index 172133e..ad3258c 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -115,6 +115,7 @@
"//aos:configuration_fbs",
"//aos:flatbuffers",
"//aos:ftrace",
+ "//aos:realtime",
"//aos:uuid",
"//aos/ipc_lib:data_alignment",
"//aos/logging:implementations",
diff --git a/aos/events/event_loop.cc b/aos/events/event_loop.cc
index 257313b..f5e9f51 100644
--- a/aos/events/event_loop.cc
+++ b/aos/events/event_loop.cc
@@ -3,6 +3,7 @@
#include "aos/configuration.h"
#include "aos/configuration_generated.h"
#include "aos/logging/implementations.h"
+#include "aos/realtime.h"
#include "glog/logging.h"
DEFINE_bool(timing_reports, true, "Publish timing reports.");
@@ -114,11 +115,7 @@
const monotonic_clock::time_point monotonic_now =
event_loop_->monotonic_now();
phased_loop_.Reset(monotonic_now);
- Reschedule(
- [this](monotonic_clock::time_point sleep_time) {
- Schedule(sleep_time);
- },
- monotonic_now);
+ Reschedule(monotonic_now);
// Reschedule here will count cycles elapsed before now, and then the
// reschedule before running the handler will count the time that elapsed
// then. So clear the count here.
@@ -622,6 +619,8 @@
context_.source_boot_uuid = boot_uuid();
}
+cpu_set_t EventLoop::DefaultAffinity() { return aos::DefaultAffinity(); }
+
void WatcherState::set_timing_report(timing::Watcher *watcher) {
watcher_ = watcher;
if (!watcher) {
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 809539a..23250e1 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -274,7 +274,7 @@
// Fetches the next message. Returns true if it fetched a new message. This
// method will only return messages sent after the Fetcher was created.
bool FetchNext() {
- const bool result = fetcher_->FetchNext();
+ const bool result = CHECK_NOTNULL(fetcher_)->FetchNext();
if (result) {
CheckChannelDataAlignment(fetcher_->context().data,
fetcher_->context().size);
@@ -286,7 +286,7 @@
// This will return the latest message regardless of if it was sent before or
// after the fetcher was created.
bool Fetch() {
- const bool result = fetcher_->Fetch();
+ const bool result = CHECK_NOTNULL(fetcher_)->Fetch();
if (result) {
CheckChannelDataAlignment(fetcher_->context().data,
fetcher_->context().size);
@@ -297,23 +297,25 @@
// Returns a pointer to the contained flatbuffer, or nullptr if there is no
// available message.
const T *get() const {
- return fetcher_->context().data != nullptr
+ return CHECK_NOTNULL(fetcher_)->context().data != nullptr
? flatbuffers::GetRoot<T>(
reinterpret_cast<const char *>(fetcher_->context().data))
: nullptr;
}
// Returns the channel this fetcher uses
- const Channel *channel() const { return fetcher_->channel(); }
+ const Channel *channel() const { return CHECK_NOTNULL(fetcher_)->channel(); }
// Returns the context holding timestamps and other metadata about the
// message.
- const Context &context() const { return fetcher_->context(); }
+ const Context &context() const { return CHECK_NOTNULL(fetcher_)->context(); }
const T &operator*() const { return *get(); }
const T *operator->() const { return get(); }
- // Returns true if this fetcher is valid and connected to a channel.
+ // Returns true if this fetcher is valid and connected to a channel. If you,
+ // e.g., are using TryMakeFetcher, then you must check valid() before
+ // attempting to use the Fetcher.
bool valid() const { return static_cast<bool>(fetcher_); }
// Copies the current flatbuffer into a FlatbufferVector.
@@ -346,7 +348,7 @@
Builder(RawSender *sender, ChannelPreallocatedAllocator *allocator)
: fbb_(allocator->size(), allocator),
allocator_(allocator),
- sender_(sender) {
+ sender_(CHECK_NOTNULL(sender)) {
CheckChannelDataAlignment(allocator->data(), allocator->size());
fbb_.ForceDefaults(true);
}
@@ -414,10 +416,13 @@
RawSender::Error SendDetached(FlatbufferDetachedBuffer<T> detached);
// Equivalent to RawSender::CheckOk
- void CheckOk(const RawSender::Error err) { sender_->CheckOk(err); };
+ void CheckOk(const RawSender::Error err) {
+ CHECK_NOTNULL(sender_)->CheckOk(err);
+ };
- // Returns the name of the underlying queue.
- const Channel *channel() const { return sender_->channel(); }
+ // Returns the name of the underlying queue, if valid. You must check valid()
+ // first.
+ const Channel *channel() const { return CHECK_NOTNULL(sender_)->channel(); }
// Returns true if the Sender is a valid Sender. If you, e.g., are using
// TryMakeSender, then you must check valid() before attempting to use the
@@ -428,17 +433,19 @@
// Returns the time_points that the last message was sent at.
aos::monotonic_clock::time_point monotonic_sent_time() const {
- return sender_->monotonic_sent_time();
+ return CHECK_NOTNULL(sender_)->monotonic_sent_time();
}
aos::realtime_clock::time_point realtime_sent_time() const {
- return sender_->realtime_sent_time();
+ return CHECK_NOTNULL(sender_)->realtime_sent_time();
}
// Returns the queue index that this was sent with.
- uint32_t sent_queue_index() const { return sender_->sent_queue_index(); }
+ uint32_t sent_queue_index() const {
+ return CHECK_NOTNULL(sender_)->sent_queue_index();
+ }
// Returns the buffer index which MakeBuilder() will expose access to. This is
// the buffer the caller can fill out.
- int buffer_index() const { return sender_->buffer_index(); }
+ int buffer_index() const { return CHECK_NOTNULL(sender_)->buffer_index(); }
private:
friend class EventLoop;
@@ -477,6 +484,9 @@
// Stop future calls to callback().
virtual void Disable() = 0;
+ // Check if the timer is disabled
+ virtual bool IsDisabled() = 0;
+
// Sets and gets the name of the timer. Set this if you want a descriptive
// name in the timing report.
void set_name(std::string_view name) { name_ = std::string(name); }
@@ -501,26 +511,61 @@
Ftrace ftrace_;
};
-// Interface for phased loops. They are built on timers.
+// Interface for phased loops. They are built on timers.
class PhasedLoopHandler {
public:
virtual ~PhasedLoopHandler();
- // Sets the interval and offset. Any changes to interval and offset only take
- // effect when the handler finishes running.
- void set_interval_and_offset(const monotonic_clock::duration interval,
- const monotonic_clock::duration offset) {
- phased_loop_.set_interval_and_offset(interval, offset);
+ // Sets the interval and offset. Any changes to interval and offset only take
+ // effect when the handler finishes running or upon a call to Reschedule().
+ //
+ // The precise semantics of the monotonic_now parameter are defined in
+ // phased_loop.h. The way that it gets used in this interface is to control
+ // what the cycles elapsed counter will read on the following call to your
+ // handler. In an idealized simulation environment, if you call
+ // set_interval_and_offset() during the phased loop offset without setting
+ // monotonic_now, then you should always see a count of 1 on the next cycle.
+ //
+ // With the default behavior (this is called inside your handler and with
+ // monotonic_now = nullopt), the next phased loop call will occur at most
+ // interval time after the current time. Note that in many cases this will
+ // *not* be the preferred behavior (in most cases, you would likely aim to
+ // keep the average frequency of the calls reasonably consistent).
+ //
+ // A combination of the monotonic_now parameter and manually calling
+ // Reschedule() outside of the phased loop handler can be used to alter the
+ // behavior of cycles_elapsed. For the default behavior, you can set
+ // monotonic_now to the current time. If you call set_interval_and_offset()
+ // and Reschedule() with the same monotonic_now, that will cause the next
+ // callback to occur in the range (monotonic_now, monotonic_now + interval]
+ // and get called with a count of 1 (if the event is actually able to happen
+ // when it is scheduled to). E.g., if you are just adjusting the phased loop
+ // offset (but not the interval) and want to maintain a consistent frequency,
+ // you may call these functions with monotonic_now = now + interval / 2.
+ void set_interval_and_offset(
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset,
+ std::optional<monotonic_clock::time_point> monotonic_now = std::nullopt) {
+ phased_loop_.set_interval_and_offset(interval, offset, monotonic_now);
}
- // Sets and gets the name of the timer. Set this if you want a descriptive
+ // Reruns the scheduler for the phased loop, scheduling the next callback at
+ // the next available time after monotonic_now. This allows
+ // set_interval_and_offset() to be called and have the changes take effect
+ // before the next handler finishes. This will have no effect if run during
+ // the phased loop's own handler.
+ void Reschedule(monotonic_clock::time_point monotonic_now) {
+ cycles_elapsed_ += phased_loop_.Iterate(monotonic_now);
+ Schedule(phased_loop_.sleep_time());
+ }
+
+ // Sets and gets the name of the timer. Set this if you want a descriptive
// name in the timing report.
void set_name(std::string_view name) { name_ = std::string(name); }
const std::string_view name() const { return name_; }
protected:
- void Call(std::function<monotonic_clock::time_point()> get_time,
- std::function<void(monotonic_clock::time_point)> schedule);
+ void Call(std::function<monotonic_clock::time_point()> get_time);
PhasedLoopHandler(EventLoop *event_loop, std::function<void(int)> fn,
const monotonic_clock::duration interval,
@@ -529,12 +574,6 @@
private:
friend class EventLoop;
- void Reschedule(std::function<void(monotonic_clock::time_point)> schedule,
- monotonic_clock::time_point monotonic_now) {
- cycles_elapsed_ += phased_loop_.Iterate(monotonic_now);
- schedule(phased_loop_.sleep_time());
- }
-
virtual void Schedule(monotonic_clock::time_point sleep_time) = 0;
EventLoop *event_loop_;
@@ -576,7 +615,8 @@
}
// Like MakeFetcher, but returns an invalid fetcher if the given channel is
- // not readable on this node or does not exist.
+ // not readable on this node or does not exist. You must check valid() on the
+ // Fetcher before using it.
template <typename T>
Fetcher<T> TryMakeFetcher(const std::string_view channel_name) {
const Channel *const channel = GetChannel<T>(channel_name);
@@ -611,7 +651,8 @@
}
// Like MakeSender, but returns an invalid sender if the given channel is
- // not sendable on this node or does not exist.
+ // not sendable on this node or does not exist. You must check valid() on the
+ // Sender before using it.
template <typename T>
Sender<T> TryMakeSender(const std::string_view channel_name) {
const Channel *channel = GetChannel<T>(channel_name);
@@ -715,13 +756,7 @@
// Defaults to 0 if this loop will not run realtime.
virtual int runtime_realtime_priority() const = 0;
- static cpu_set_t DefaultAffinity() {
- cpu_set_t result;
- for (int i = 0; i < CPU_SETSIZE; ++i) {
- CPU_SET(i, &result);
- }
- return result;
- }
+ static cpu_set_t DefaultAffinity();
// Sets the scheduler affinity to run the event loop with. This may only be
// called before Run().
diff --git a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc
index 990cbe5..07bd6d4 100644
--- a/aos/events/event_loop_param_test.cc
+++ b/aos/events/event_loop_param_test.cc
@@ -524,6 +524,104 @@
EXPECT_THAT(values, ::testing::ElementsAreArray({201}));
}
+// Tests that timer handler is enabled after setup (even if it is in the past)
+// and is disabled after running
+TEST_P(AbstractEventLoopTest, CheckTimerDisabled) {
+ auto loop = MakePrimary("primary");
+
+ auto timer = loop->AddTimer([this]() {
+ LOG(INFO) << "timer called";
+ Exit();
+ });
+
+ loop->OnRun([&loop, timer]() {
+ EXPECT_TRUE(timer->IsDisabled());
+ timer->Setup(loop->monotonic_now() + chrono::milliseconds(100));
+ EXPECT_FALSE(timer->IsDisabled());
+ });
+
+ Run();
+ EXPECT_TRUE(timer->IsDisabled());
+}
+
+// Tests that timer handler is enabled after setup (even if it is in the past)
+// and is disabled after running
+TEST_P(AbstractEventLoopTest, CheckTimerRunInPastDisabled) {
+ auto loop = MakePrimary("primary");
+
+ auto timer2 = loop->AddTimer([this]() {
+ LOG(INFO) << "timer called";
+ Exit();
+ });
+
+ auto timer = loop->AddTimer([&loop, timer2]() {
+ timer2->Setup(loop->monotonic_now() - chrono::nanoseconds(1));
+ });
+
+ loop->OnRun([&loop, timer]() {
+ timer->Setup(loop->monotonic_now() + chrono::seconds(1));
+ EXPECT_FALSE(timer->IsDisabled());
+ });
+
+ Run();
+ EXPECT_TRUE(timer2->IsDisabled());
+}
+
+// Tests that timer handler is not disabled even after calling Exit on the event
+// loop within the timer
+TEST_P(AbstractEventLoopTest, CheckTimerRepeatOnCountDisabled) {
+ auto loop = MakePrimary("primary");
+ int counter = 0;
+
+ auto timer = loop->AddTimer([&counter, this]() {
+ LOG(INFO) << "timer called";
+ counter++;
+ if (counter >= 5) {
+ Exit();
+ }
+ });
+
+ loop->OnRun([&loop, timer]() {
+ timer->Setup(loop->monotonic_now() + chrono::seconds(1),
+ chrono::seconds(1));
+ EXPECT_FALSE(timer->IsDisabled());
+ });
+ Run();
+
+ // Sanity check
+ EXPECT_EQ(counter, 5);
+
+ // if you run the loop again, the timer will start running again
+ EXPECT_FALSE(timer->IsDisabled());
+
+ counter = 0;
+ Run();
+ timer->Disable();
+
+ EXPECT_TRUE(timer->IsDisabled());
+}
+
+// Tests that timer handler is not disabled even after calling Exit on the event
+// loop using an external timer
+TEST_P(AbstractEventLoopTest, CheckTimerRepeatTillEndTimerDisabled) {
+ auto loop = MakePrimary("primary");
+
+ auto timer = loop->AddTimer([]() { LOG(INFO) << "timer called"; });
+
+ loop->OnRun([&loop, timer]() {
+ timer->Setup(loop->monotonic_now() + chrono::seconds(1),
+ chrono::seconds(1));
+ EXPECT_FALSE(timer->IsDisabled());
+ });
+
+ EndEventLoop(loop.get(), chrono::seconds(5));
+ Run();
+ EXPECT_FALSE(timer->IsDisabled());
+
+ timer->Disable();
+ EXPECT_TRUE(timer->IsDisabled());
+}
+
// Tests that Fetch and FetchNext interleave as expected.
TEST_P(AbstractEventLoopTest, FetchAndFetchNextTogether) {
auto loop1 = Make();
@@ -1824,6 +1922,298 @@
EXPECT_GT(expected_times[expected_times.size() / 2], average_time - kEpsilon);
}
+// Tests that a phased loop responds correctly to a changing offset; sweep
+// across a variety of potential offset changes, to ensure that we are
+// exercising a variety of potential cases.
+TEST_P(AbstractEventLoopTest, PhasedLoopChangingOffsetSweep) {
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+ const int kCount = 5;
+
+ auto loop1 = MakePrimary();
+
+ std::vector<aos::monotonic_clock::duration> offset_options;
+ for (int ii = 0; ii < kCount; ++ii) {
+ offset_options.push_back(ii * kInterval / kCount);
+ }
+ std::vector<aos::monotonic_clock::duration> offset_sweep;
+ // Run over all the pair-wise combinations of offsets.
+ for (int ii = 0; ii < kCount; ++ii) {
+ for (int jj = 0; jj < kCount; ++jj) {
+ offset_sweep.push_back(offset_options.at(ii));
+ offset_sweep.push_back(offset_options.at(jj));
+ }
+ }
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ // Run kCount iterations.
+ size_t counter = 0;
+ phased_loop = loop1->AddPhasedLoop(
+ [&phased_loop, &expected_times, &loop1, this, kInterval, &counter,
+ offset_sweep](int count) {
+ EXPECT_EQ(count, 1);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ counter++;
+
+ if (counter == offset_sweep.size()) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+
+ phased_loop->set_interval_and_offset(kInterval,
+ offset_sweep.at(counter));
+ },
+ kInterval, offset_sweep.at(0));
+
+ Run();
+ ASSERT_EQ(expected_times.size(), offset_sweep.size());
+ for (size_t ii = 1; ii < expected_times.size(); ++ii) {
+ EXPECT_LE(expected_times.at(ii) - expected_times.at(ii - 1), kInterval);
+ }
+}
+
+// Tests that a phased loop responds correctly to being rescheduled with now
+// equal to a time in the past.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleInPast) {
+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+ auto loop1 = MakePrimary();
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ int expected_count = 1;
+
+ // Set up a timer that will get run immediately after the phased loop and
+ // which will attempt to reschedule the phased loop to just before now. This
+ // should succeed, but will result in 0 cycles elapsing.
+ TimerHandler *manager_timer =
+ loop1->AddTimer([&phased_loop, &loop1, &expected_count, this]() {
+ if (expected_count == 0) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+ phased_loop->Reschedule(loop1->context().monotonic_event_time -
+ std::chrono::nanoseconds(1));
+ expected_count = 0;
+ });
+
+ phased_loop = loop1->AddPhasedLoop(
+ [&expected_count, &expected_times, &loop1, manager_timer](int count) {
+ EXPECT_EQ(count, expected_count);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ manager_timer->Setup(loop1->context().monotonic_event_time);
+ },
+ kInterval, kOffset);
+ phased_loop->set_name("Test loop");
+ manager_timer->set_name("Manager timer");
+
+ Run();
+
+ ASSERT_EQ(2u, expected_times.size());
+ ASSERT_EQ(expected_times[0], expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to being rescheduled at the time
+// when it should be triggering (it should kick the trigger to the next cycle).
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleNow) {
+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+ auto loop1 = MakePrimary();
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ bool should_exit = false;
+ // Set up a timer that will get run immediately after the phased loop and
+ // which will attempt to reschedule the phased loop to now. This should
+ // succeed, but will result in no change to the expected behavior (since this
+ // is the same thing that is actually done internally).
+ TimerHandler *manager_timer =
+ loop1->AddTimer([&phased_loop, &loop1, &should_exit, this]() {
+ if (should_exit) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+ phased_loop->Reschedule(loop1->context().monotonic_event_time);
+ should_exit = true;
+ });
+
+ phased_loop = loop1->AddPhasedLoop(
+ [&expected_times, &loop1, manager_timer](int count) {
+ EXPECT_EQ(count, 1);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ manager_timer->Setup(loop1->context().monotonic_event_time);
+ },
+ kInterval, kOffset);
+ phased_loop->set_name("Test loop");
+ manager_timer->set_name("Manager timer");
+
+ Run();
+
+ ASSERT_EQ(2u, expected_times.size());
+ ASSERT_EQ(expected_times[0] + kInterval, expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to being rescheduled at a time in
+// the distant future.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleFuture) {
+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+ auto loop1 = MakePrimary();
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ bool should_exit = false;
+ int expected_count = 1;
+ TimerHandler *manager_timer = loop1->AddTimer(
+ [&expected_count, &phased_loop, &loop1, &should_exit, this, kInterval]() {
+ if (should_exit) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+ expected_count = 10;
+ // Knock off 1 ns, since the scheduler rounds up when it is
+ // scheduled to exactly a loop time.
+ phased_loop->Reschedule(loop1->context().monotonic_event_time +
+ kInterval * expected_count -
+ std::chrono::nanoseconds(1));
+ should_exit = true;
+ });
+
+ phased_loop = loop1->AddPhasedLoop(
+ [&expected_times, &loop1, manager_timer, &expected_count](int count) {
+ EXPECT_EQ(count, expected_count);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ manager_timer->Setup(loop1->context().monotonic_event_time);
+ },
+ kInterval, kOffset);
+ phased_loop->set_name("Test loop");
+ manager_timer->set_name("Manager timer");
+
+ Run();
+
+ ASSERT_EQ(2u, expected_times.size());
+ ASSERT_EQ(expected_times[0] + expected_count * kInterval, expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to having its phase offset
+// incremented and then being scheduled after a set time, exercising a pattern
+// where a phased loop's offset is changed while trying to maintain the trigger
+// at a consistent period.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithLaterOffset) {
+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+ auto loop1 = MakePrimary();
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ bool should_exit = false;
+ TimerHandler *manager_timer = loop1->AddTimer(
+ [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
+ if (should_exit) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+ // Schedule the next callback to be strictly later than the current time
+ // + interval / 2, to ensure a consistent frequency.
+ monotonic_clock::time_point half_time =
+ loop1->context().monotonic_event_time + kInterval / 2;
+ phased_loop->set_interval_and_offset(
+ kInterval, kOffset + std::chrono::nanoseconds(1), half_time);
+ phased_loop->Reschedule(half_time);
+ should_exit = true;
+ });
+
+ phased_loop = loop1->AddPhasedLoop(
+ [&expected_times, &loop1, manager_timer](int count) {
+ EXPECT_EQ(1, count);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ manager_timer->Setup(loop1->context().monotonic_event_time);
+ },
+ kInterval, kOffset);
+ phased_loop->set_name("Test loop");
+ manager_timer->set_name("Manager timer");
+
+ Run();
+
+ ASSERT_EQ(2u, expected_times.size());
+ ASSERT_EQ(expected_times[0] + kInterval + std::chrono::nanoseconds(1),
+ expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to having its phase offset
+// decremented and then being scheduled after a set time, exercising a pattern
+// where a phased loop's offset is changed while trying to maintain the trigger
+// at a consistent period.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithEarlierOffset) {
+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+ auto loop1 = MakePrimary();
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ bool should_exit = false;
+ TimerHandler *manager_timer = loop1->AddTimer(
+ [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
+ if (should_exit) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+ // Schedule the next callback to be strictly later than the current time
+ // + interval / 2, to ensure a consistent frequency.
+ const aos::monotonic_clock::time_point half_time =
+ loop1->context().monotonic_event_time + kInterval / 2;
+ phased_loop->set_interval_and_offset(
+ kInterval, kOffset - std::chrono::nanoseconds(1), half_time);
+ phased_loop->Reschedule(half_time);
+ should_exit = true;
+ });
+
+ phased_loop = loop1->AddPhasedLoop(
+ [&expected_times, &loop1, manager_timer](int count) {
+ EXPECT_EQ(1, count);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ manager_timer->Setup(loop1->context().monotonic_event_time);
+ },
+ kInterval, kOffset);
+ phased_loop->set_name("Test loop");
+ manager_timer->set_name("Manager timer");
+
+ Run();
+
+ ASSERT_EQ(2u, expected_times.size());
+ ASSERT_EQ(expected_times[0] + kInterval - std::chrono::nanoseconds(1),
+ expected_times[1]);
+}
+
// Tests that senders count correctly in the timing report.
TEST_P(AbstractEventLoopTest, SenderTimingReport) {
FLAGS_timing_report_ms = 1000;
diff --git a/aos/events/event_loop_param_test.cc.rej b/aos/events/event_loop_param_test.cc.rej
new file mode 100644
index 0000000..c03b83d
--- /dev/null
+++ b/aos/events/event_loop_param_test.cc.rej
@@ -0,0 +1,300 @@
+diff a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc (rejected hunks)
+@@ -1923,6 +1923,298 @@
+ EXPECT_GT(expected_times[expected_times.size() / 2], average_time - kEpsilon);
+ }
+
++// Tests that a phased loop responds correctly to a changing offset; sweep
++// across a variety of potential offset changes, to ensure that we are
++// exercising a variety of potential cases.
++TEST_P(AbstractEventLoopTest, PhasedLoopChangingOffsetSweep) {
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++ const int kCount = 5;
++
++ auto loop1 = MakePrimary();
++
++ std::vector<aos::monotonic_clock::duration> offset_options;
++ for (int ii = 0; ii < kCount; ++ii) {
++ offset_options.push_back(ii * kInterval / kCount);
++ }
++ std::vector<aos::monotonic_clock::duration> offset_sweep;
++ // Run over all the pair-wise combinations of offsets.
++ for (int ii = 0; ii < kCount; ++ii) {
++ for (int jj = 0; jj < kCount; ++jj) {
++ offset_sweep.push_back(offset_options.at(ii));
++ offset_sweep.push_back(offset_options.at(jj));
++ }
++ }
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ // Run kCount iterations.
++ int counter = 0;
++ phased_loop = loop1->AddPhasedLoop(
++ [&phased_loop, &expected_times, &loop1, this, kInterval, &counter,
++ offset_sweep](int count) {
++ EXPECT_EQ(count, 1);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ counter++;
++
++ if (counter == offset_sweep.size()) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++
++ phased_loop->set_interval_and_offset(kInterval,
++ offset_sweep.at(counter));
++ },
++ kInterval, offset_sweep.at(0));
++
++ Run();
++ ASSERT_EQ(expected_times.size(), offset_sweep.size());
++ for (size_t ii = 1; ii < expected_times.size(); ++ii) {
++ EXPECT_LE(expected_times.at(ii) - expected_times.at(ii - 1), kInterval);
++ }
++}
++
++// Tests that a phased loop responds correctly to being rescheduled with now
++// equal to a time in the past.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleInPast) {
++ const chrono::milliseconds kOffset = chrono::milliseconds(400);
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++ auto loop1 = MakePrimary();
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ int expected_count = 1;
++
++ // Set up a timer that will get run immediately after the phased loop and
++ // which will attempt to reschedule the phased loop to just before now. This
++ // should succeed, but will result in 0 cycles elapsing.
++ TimerHandler *manager_timer =
++ loop1->AddTimer([&phased_loop, &loop1, &expected_count, this]() {
++ if (expected_count == 0) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++ phased_loop->Reschedule(loop1->context().monotonic_event_time -
++ std::chrono::nanoseconds(1));
++ expected_count = 0;
++ });
++
++ phased_loop = loop1->AddPhasedLoop(
++ [&expected_count, &expected_times, &loop1, manager_timer](int count) {
++ EXPECT_EQ(count, expected_count);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ manager_timer->Setup(loop1->context().monotonic_event_time);
++ },
++ kInterval, kOffset);
++ phased_loop->set_name("Test loop");
++ manager_timer->set_name("Manager timer");
++
++ Run();
++
++ ASSERT_EQ(2u, expected_times.size());
++ ASSERT_EQ(expected_times[0], expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to being rescheduled at the time
++// when it should be triggering (it should kick the trigger to the next cycle).
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleNow) {
++ const chrono::milliseconds kOffset = chrono::milliseconds(400);
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++ auto loop1 = MakePrimary();
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ bool should_exit = false;
++ // Set up a timer that will get run immediately after the phased loop and
++ // which will attempt to reschedule the phased loop to now. This should
++ // succeed, but will result in no change to the expected behavior (since this
++ // is the same thing that is actually done internally).
++ TimerHandler *manager_timer =
++ loop1->AddTimer([&phased_loop, &loop1, &should_exit, this]() {
++ if (should_exit) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++ phased_loop->Reschedule(loop1->context().monotonic_event_time);
++ should_exit = true;
++ });
++
++ phased_loop = loop1->AddPhasedLoop(
++ [&expected_times, &loop1, manager_timer](int count) {
++ EXPECT_EQ(count, 1);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ manager_timer->Setup(loop1->context().monotonic_event_time);
++ },
++ kInterval, kOffset);
++ phased_loop->set_name("Test loop");
++ manager_timer->set_name("Manager timer");
++
++ Run();
++
++ ASSERT_EQ(2u, expected_times.size());
++ ASSERT_EQ(expected_times[0] + kInterval, expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to being rescheduled at a time in
++// the distant future.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleFuture) {
++ const chrono::milliseconds kOffset = chrono::milliseconds(400);
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++ auto loop1 = MakePrimary();
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ bool should_exit = false;
++ int expected_count = 1;
++ TimerHandler *manager_timer = loop1->AddTimer(
++ [&expected_count, &phased_loop, &loop1, &should_exit, this, kInterval]() {
++ if (should_exit) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++ expected_count = 10;
++ // Knock off 1 ns, since the scheduler rounds up when it is
++ // scheduled to exactly a loop time.
++ phased_loop->Reschedule(loop1->context().monotonic_event_time +
++ kInterval * expected_count -
++ std::chrono::nanoseconds(1));
++ should_exit = true;
++ });
++
++ phased_loop = loop1->AddPhasedLoop(
++ [&expected_times, &loop1, manager_timer, &expected_count](int count) {
++ EXPECT_EQ(count, expected_count);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ manager_timer->Setup(loop1->context().monotonic_event_time);
++ },
++ kInterval, kOffset);
++ phased_loop->set_name("Test loop");
++ manager_timer->set_name("Manager timer");
++
++ Run();
++
++ ASSERT_EQ(2u, expected_times.size());
++ ASSERT_EQ(expected_times[0] + expected_count * kInterval, expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to having its phase offset
++// incremented and then being scheduled after a set time, exercising a pattern
++// where a phased loop's offset is changed while trying to maintain the trigger
++// at a consistent period.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithLaterOffset) {
++ const chrono::milliseconds kOffset = chrono::milliseconds(400);
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++ auto loop1 = MakePrimary();
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ bool should_exit = false;
++ TimerHandler *manager_timer = loop1->AddTimer(
++ [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
++ if (should_exit) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++ // Schedule the next callback to be strictly later than the current time
++ // + interval / 2, to ensure a consistent frequency.
++ monotonic_clock::time_point half_time =
++ loop1->context().monotonic_event_time + kInterval / 2;
++ phased_loop->set_interval_and_offset(
++ kInterval, kOffset + std::chrono::nanoseconds(1), half_time);
++ phased_loop->Reschedule(half_time);
++ should_exit = true;
++ });
++
++ phased_loop = loop1->AddPhasedLoop(
++ [&expected_times, &loop1, manager_timer](int count) {
++ EXPECT_EQ(1, count);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ manager_timer->Setup(loop1->context().monotonic_event_time);
++ },
++ kInterval, kOffset);
++ phased_loop->set_name("Test loop");
++ manager_timer->set_name("Manager timer");
++
++ Run();
++
++ ASSERT_EQ(2u, expected_times.size());
++ ASSERT_EQ(expected_times[0] + kInterval + std::chrono::nanoseconds(1),
++ expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to having its phase offset
++// decremented and then being scheduled after a set time, exercising a pattern
++// where a phased loop's offset is changed while trying to maintain the trigger
++// at a consistent period.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithEarlierOffset) {
++ const chrono::milliseconds kOffset = chrono::milliseconds(400);
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++ auto loop1 = MakePrimary();
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ bool should_exit = false;
++ TimerHandler *manager_timer = loop1->AddTimer(
++ [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
++ if (should_exit) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++ // Schedule the next callback to be strictly later than the current time
++ // + interval / 2, to ensure a consistent frequency.
++ const aos::monotonic_clock::time_point half_time =
++ loop1->context().monotonic_event_time + kInterval / 2;
++ phased_loop->set_interval_and_offset(
++ kInterval, kOffset - std::chrono::nanoseconds(1), half_time);
++ phased_loop->Reschedule(half_time);
++ should_exit = true;
++ });
++
++ phased_loop = loop1->AddPhasedLoop(
++ [&expected_times, &loop1, manager_timer](int count) {
++ EXPECT_EQ(1, count);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ manager_timer->Setup(loop1->context().monotonic_event_time);
++ },
++ kInterval, kOffset);
++ phased_loop->set_name("Test loop");
++ manager_timer->set_name("Manager timer");
++
++ Run();
++
++ ASSERT_EQ(2u, expected_times.size());
++ ASSERT_EQ(expected_times[0] + kInterval - std::chrono::nanoseconds(1),
++ expected_times[1]);
++}
++
+ // Tests that senders count correctly in the timing report.
+ TEST_P(AbstractEventLoopTest, SenderTimingReport) {
+ gflags::FlagSaver flag_saver;
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
index d206bc1..39ab43e 100644
--- a/aos/events/event_loop_tmpl.h
+++ b/aos/events/event_loop_tmpl.h
@@ -238,8 +238,7 @@
}
inline void PhasedLoopHandler::Call(
- std::function<monotonic_clock::time_point()> get_time,
- std::function<void(monotonic_clock::time_point)> schedule) {
+ std::function<monotonic_clock::time_point()> get_time) {
// Read time directly to save a vtable indirection...
const monotonic_clock::time_point monotonic_start_time = get_time();
@@ -270,7 +269,7 @@
cycles_elapsed_ = 0;
// Schedule the next wakeup.
- schedule(phased_loop_.sleep_time());
+ Schedule(phased_loop_.sleep_time());
const monotonic_clock::time_point monotonic_end_time = get_time();
ftrace_.FormatMessage(
@@ -287,7 +286,7 @@
// If the handler took too long so we blew by the previous deadline, we
// want to just try for the next deadline. Reschedule.
if (monotonic_end_time > phased_loop_.sleep_time()) {
- Reschedule(schedule, monotonic_end_time);
+ Reschedule(monotonic_end_time);
}
}
diff --git a/aos/events/logging/BUILD b/aos/events/logging/BUILD
index 7e8daf9..364cabb 100644
--- a/aos/events/logging/BUILD
+++ b/aos/events/logging/BUILD
@@ -27,6 +27,17 @@
visibility = ["//visibility:public"],
)
+flatbuffer_cc_library(
+ name = "replay_config_fbs",
+ srcs = ["log_replayer_config.fbs"],
+ gen_reflections = True,
+ includes = [
+ "//aos:configuration_fbs_includes",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
cc_library(
name = "boot_timestamp",
srcs = ["boot_timestamp.cc"],
@@ -38,6 +49,51 @@
],
)
+cc_binary(
+ name = "log_replayer",
+ srcs = [
+ "log_replayer.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":log_reader",
+ ":log_reader_utils",
+ ":replay_config_fbs",
+ ":replay_timing_fbs",
+ ":replay_timing_schema",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "//aos/events:shm_event_loop",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_flatbuffers//:flatbuffers",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_library(
+ name = "log_reader_utils",
+ srcs = [
+ "log_reader_utils.cc",
+ ],
+ hdrs = [
+ "log_reader_utils.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":log_reader",
+ "//aos:configuration",
+ "//aos:flatbuffer_merge",
+ "//aos:flatbuffers",
+ "//aos:uuid",
+ "//aos/containers:resizeable_buffer",
+ "//aos/events:event_loop",
+ "//aos/util:file",
+ "@com_google_absl//absl/strings",
+ ],
+)
+
cc_library(
name = "logfile_utils",
srcs = [
@@ -543,6 +599,7 @@
name = "realtime_replay_test",
srcs = ["realtime_replay_test.cc"],
data = [
+ ":multinode_pingpong_combined_config",
"//aos/events:pingpong_config",
],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/aos/events/logging/log_edit.cc b/aos/events/logging/log_edit.cc
index e3cfc3a..dca56bb 100644
--- a/aos/events/logging/log_edit.cc
+++ b/aos/events/logging/log_edit.cc
@@ -51,7 +51,10 @@
aos::logger::DetachedBufferWriter buffer_writer(
FLAGS_logfile,
std::make_unique<aos::logger::DummyEncoder>(FLAGS_max_message_size));
- buffer_writer.QueueSpan(header.span());
+ {
+ aos::logger::DataEncoder::SpanCopier coppier(header.span());
+ buffer_writer.CopyMessage(&coppier, aos::monotonic_clock::min_time);
+ }
while (true) {
absl::Span<const uint8_t> msg_data = span_reader.ReadMessage();
@@ -59,7 +62,10 @@
break;
}
- buffer_writer.QueueSpan(msg_data);
+ {
+ aos::logger::DataEncoder::SpanCopier coppier(msg_data);
+ buffer_writer.CopyMessage(&coppier, aos::monotonic_clock::min_time);
+ }
}
} else {
aos::logger::MessageReader reader(FLAGS_logfile);
diff --git a/aos/events/logging/log_namer.cc b/aos/events/logging/log_namer.cc
index 120bd79..c0c7c73 100644
--- a/aos/events/logging/log_namer.cc
+++ b/aos/events/logging/log_namer.cc
@@ -248,7 +248,8 @@
header, {.multi_line = false, .max_vector_size = 100});
CHECK(writer);
- writer->QueueSpan(header.span());
+ DataEncoder::SpanCopier coppier(header.span());
+ writer->CopyMessage(&coppier, aos::monotonic_clock::now());
header_written_ = true;
monotonic_start_time_ = log_namer_->monotonic_start_time(
node_index_, state_[node_index_].boot_uuid);
@@ -606,7 +607,8 @@
std::make_unique<DetachedBufferWriter>(
filename, encoder_factory_(header->span().size()));
- writer->QueueSpan(header->span());
+ DataEncoder::SpanCopier coppier(header->span());
+ writer->CopyMessage(&coppier, aos::monotonic_clock::now());
if (!writer->ran_out_of_space()) {
all_filenames_.emplace_back(
diff --git a/aos/events/logging/log_reader.cc b/aos/events/logging/log_reader.cc
index f80f28d..d3ce16b 100644
--- a/aos/events/logging/log_reader.cc
+++ b/aos/events/logging/log_reader.cc
@@ -233,13 +233,17 @@
};
LogReader::LogReader(std::string_view filename,
- const Configuration *replay_configuration)
- : LogReader(SortParts({std::string(filename)}), replay_configuration) {}
+ const Configuration *replay_configuration,
+ const ReplayChannels *replay_channels)
+ : LogReader(SortParts({std::string(filename)}), replay_configuration,
+ replay_channels) {}
LogReader::LogReader(std::vector<LogFile> log_files,
- const Configuration *replay_configuration)
+ const Configuration *replay_configuration,
+ const ReplayChannels *replay_channels)
: log_files_(std::move(log_files)),
- replay_configuration_(replay_configuration) {
+ replay_configuration_(replay_configuration),
+ replay_channels_(replay_channels) {
SetStartTime(FLAGS_start_time);
SetEndTime(FLAGS_end_time);
@@ -260,6 +264,11 @@
}
}
+ if (replay_channels_ != nullptr) {
+ CHECK(!replay_channels_->empty()) << "replay_channels is empty which means "
+ "no messages will get replayed.";
+ }
+
MakeRemappedConfig();
// Remap all existing remote timestamp channels. They will be recreated, and
@@ -444,6 +453,7 @@
std::nullopt, false,
last_queued_message_ == BootTimestamp::max_time()};
}
+
TimestampedMessage *message = timestamp_mapper_->Front();
// Upon reaching the end of the log, exit.
if (message == nullptr) {
@@ -452,6 +462,7 @@
BootTimestamp>::PushResult{std::nullopt,
false, true};
}
+
last_queued_message_ = message->monotonic_event_time;
const util::ThreadedQueue<TimestampedMessage,
BootTimestamp>::PushResult result{
@@ -606,7 +617,8 @@
filtered_parts.size() == 0u
? nullptr
: std::make_unique<TimestampMapper>(std::move(filtered_parts)),
- filters_.get(), node, State::ThreadedBuffering::kNo);
+ filters_.get(), node, State::ThreadedBuffering::kNo,
+ MaybeMakeReplayChannelIndicies(node));
State *state = states_[node_index].get();
state->SetNodeEventLoopFactory(
event_loop_factory_->GetNodeEventLoopFactory(node),
@@ -647,6 +659,7 @@
if (state->SingleThreadedOldestMessageTime() == BootTimestamp::max_time()) {
continue;
}
+
++live_nodes_;
NodeEventLoopFactory *node_factory =
@@ -795,7 +808,8 @@
filtered_parts.size() == 0u
? nullptr
: std::make_unique<TimestampMapper>(std::move(filtered_parts)),
- filters_.get(), node, State::ThreadedBuffering::kYes);
+ filters_.get(), node, State::ThreadedBuffering::kYes,
+ MaybeMakeReplayChannelIndicies(node));
State *state = states_[node_index].get();
state->SetChannelCount(logged_configuration()->channels()->size());
@@ -1316,6 +1330,13 @@
std::string_view add_prefix,
std::string_view new_type,
RemapConflict conflict_handling) {
+ if (replay_channels_ != nullptr) {
+ CHECK(std::find(replay_channels_->begin(), replay_channels_->end(),
+ std::make_pair(name, type)) != replay_channels_->end())
+ << "Attempted to remap channel " << name << " " << type
+ << " which is not included in the replay channels passed to LogReader.";
+ }
+
for (size_t ii = 0; ii < logged_configuration()->channels()->size(); ++ii) {
const Channel *const channel = logged_configuration()->channels()->Get(ii);
if (channel->name()->str() == name &&
@@ -1646,6 +1667,31 @@
// TODO(austin): Lazily re-build to save CPU?
}
+std::unique_ptr<const ReplayChannelIndicies>
+LogReader::MaybeMakeReplayChannelIndicies(const Node *node) {
+ if (replay_channels_ == nullptr) {
+ return nullptr;
+ } else {
+ std::unique_ptr<ReplayChannelIndicies> replay_channel_indicies =
+ std::make_unique<ReplayChannelIndicies>();
+ for (auto const &channel : *replay_channels_) {
+ const Channel *ch = configuration::GetChannel(
+ logged_configuration(), channel.first, channel.second, "", node);
+ if (ch == nullptr) {
+ LOG(WARNING) << "Channel: " << channel.first << " " << channel.second
+ << " not found in configuration for node: "
+ << node->name()->string_view() << " Skipping ...";
+ continue;
+ }
+ const size_t channel_index =
+ configuration::ChannelIndex(logged_configuration(), ch);
+ replay_channel_indicies->emplace_back(channel_index);
+ }
+ std::sort(replay_channel_indicies->begin(), replay_channel_indicies->end());
+ return replay_channel_indicies;
+ }
+}
+
std::vector<const Channel *> LogReader::RemappedChannels() const {
std::vector<const Channel *> result;
result.reserve(remapped_channels_.size());
@@ -1699,11 +1745,24 @@
LogReader::State::State(
std::unique_ptr<TimestampMapper> timestamp_mapper,
message_bridge::MultiNodeNoncausalOffsetEstimator *multinode_filters,
- const Node *node, LogReader::State::ThreadedBuffering threading)
+ const Node *node, LogReader::State::ThreadedBuffering threading,
+ std::unique_ptr<const ReplayChannelIndicies> replay_channel_indicies)
: timestamp_mapper_(std::move(timestamp_mapper)),
node_(node),
multinode_filters_(multinode_filters),
- threading_(threading) {}
+ threading_(threading),
+ replay_channel_indicies_(std::move(replay_channel_indicies)) {
+ if (replay_channel_indicies_ != nullptr) {
+ timestamp_mapper_->set_replay_channels_callback(
+ [filter = replay_channel_indicies_.get()](
+ const TimestampedMessage &message) -> bool {
+ auto const begin = filter->cbegin();
+ auto const end = filter->cend();
+ // TODO: benchmark strategies for channel_index matching
+ return std::binary_search(begin, end, message.channel_index);
+ });
+ }
+}
void LogReader::State::AddPeer(State *peer) {
if (timestamp_mapper_ && peer->timestamp_mapper_) {
@@ -2131,6 +2190,7 @@
}
TimestampedMessage LogReader::State::PopOldest() {
+ // multithreaded
if (message_queuer_.has_value()) {
std::optional<TimestampedMessage> message = message_queuer_->Pop();
CHECK(message.has_value()) << ": Unexpectedly ran out of messages.";
@@ -2139,7 +2199,7 @@
std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(FLAGS_threaded_look_ahead_seconds)));
return message.value();
- } else {
+ } else { // single threaded
CHECK(timestamp_mapper_ != nullptr);
TimestampedMessage *result_ptr = timestamp_mapper_->Front();
CHECK(result_ptr != nullptr);
diff --git a/aos/events/logging/log_reader.h b/aos/events/logging/log_reader.h
index 07cca48..0d50fb9 100644
--- a/aos/events/logging/log_reader.h
+++ b/aos/events/logging/log_reader.h
@@ -31,6 +31,12 @@
class EventNotifier;
+// Vector of pair of name and type of the channel
+using ReplayChannels =
+ std::vector<std::pair<std::string_view, std::string_view>>;
+// Vector of channel indices
+using ReplayChannelIndicies = std::vector<size_t>;
+
// We end up with one of the following 3 log file types.
//
// Single node logged as the source node.
@@ -67,11 +73,16 @@
// pass it in here. It must provide all the channels that the original logged
// config did.
//
+ // If certain messages should not be replayed, the replay_channels param can
+ // be used as an inclusive list of channels for messages to be replayed.
+ //
// The single file constructor calls SortParts internally.
LogReader(std::string_view filename,
- const Configuration *replay_configuration = nullptr);
+ const Configuration *replay_configuration = nullptr,
+ const ReplayChannels *replay_channels = nullptr);
LogReader(std::vector<LogFile> log_files,
- const Configuration *replay_configuration = nullptr);
+ const Configuration *replay_configuration = nullptr,
+ const ReplayChannels *replay_channels = nullptr);
~LogReader();
// Registers all the callbacks to send the log file data out on an event loop
@@ -332,7 +343,8 @@
enum class ThreadedBuffering { kYes, kNo };
State(std::unique_ptr<TimestampMapper> timestamp_mapper,
message_bridge::MultiNodeNoncausalOffsetEstimator *multinode_filters,
- const Node *node, ThreadedBuffering threading);
+ const Node *node, ThreadedBuffering threading,
+ std::unique_ptr<const ReplayChannelIndicies> replay_channel_indicies);
// Connects up the timestamp mappers.
void AddPeer(State *peer);
@@ -728,8 +740,18 @@
std::optional<BootTimestamp> last_queued_message_;
std::optional<util::ThreadedQueue<TimestampedMessage, BootTimestamp>>
message_queuer_;
+
+ // If a ReplayChannels was passed to LogReader, this will hold the
+ // indices of the channels to replay for the Node represented by
+ // the instance of LogReader::State.
+ std::unique_ptr<const ReplayChannelIndicies> replay_channel_indicies_;
};
+ // If a ReplayChannels was passed to LogReader then creates a
+ // ReplayChannelIndicies for the given node. Otherwise, returns a nullptr.
+ std::unique_ptr<const ReplayChannelIndicies> MaybeMakeReplayChannelIndicies(
+ const Node *node);
+
// Node index -> State.
std::vector<std::unique_ptr<State>> states_;
@@ -766,6 +788,10 @@
const Configuration *remapped_configuration_ = nullptr;
const Configuration *replay_configuration_ = nullptr;
+ // If a ReplayChannels was passed to LogReader, this will hold the
+ // name and type of channels to replay which is used when creating States.
+ const ReplayChannels *replay_channels_ = nullptr;
+
// If true, the replay timer will ignore any missing data. This is used
// during startup when we are bootstrapping everything and trying to get to
// the start of all the log files.
diff --git a/aos/events/logging/log_reader_utils.cc b/aos/events/logging/log_reader_utils.cc
new file mode 100644
index 0000000..295f092
--- /dev/null
+++ b/aos/events/logging/log_reader_utils.cc
@@ -0,0 +1,232 @@
+#include "aos/events/logging/log_reader_utils.h"
+
+#include "absl/strings/str_join.h"
+
+namespace {
+
+struct ChannelsExtractorAccumulator {
+ // A set of senders, watchers and fetchers to have unique channels
+ std::set<int> senders;
+ std::set<int> watchers;
+ std::set<int> fetchers;
+
+ // observed_applications are all the applications for which timing reports
+ // have been found
+ std::vector<std::set<std::string>> observed_applications;
+
+ // remaining_applications are the vector of applications that have not been
+ // found on the nodes specified
+ std::vector<std::set<std::string>> remaining_applications;
+};
+
+void HandleChannelsInApplications(
+ const aos::timing::Report &report, const size_t nodes_index,
+ aos::SimulatedEventLoopFactory *factory,
+ const aos::Configuration *logged_configuration,
+ const aos::logger::ChannelsInLogOptions &options,
+ ChannelsExtractorAccumulator *results) {
+ std::string name = report.name()->str();
+ if (results->observed_applications[nodes_index].count(name) > 0) {
+ if (!results->remaining_applications[nodes_index].empty()) {
+ LOG(FATAL) << "Didn't see timing reports for every application! "
+ << absl::StrJoin(results->remaining_applications[nodes_index],
+ ", ");
+ } else {
+ factory->Exit();
+ }
+ }
+ if (results->remaining_applications[nodes_index].count(name) == 0) {
+ return;
+ }
+ results->observed_applications[nodes_index].insert(name);
+ results->remaining_applications[nodes_index].erase(name);
+
+ if (options.get_senders) {
+ if (report.has_senders()) {
+ for (const aos::timing::Sender *sender : *report.senders()) {
+ CHECK_LT(0, sender->channel_index());
+ CHECK_LT(static_cast<size_t>(sender->channel_index()),
+ logged_configuration->channels()->size());
+ results->senders.insert(sender->channel_index());
+ }
+ }
+ }
+
+ if (options.get_watchers) {
+ if (report.has_watchers()) {
+ for (const aos::timing::Watcher *watcher : *report.watchers()) {
+ CHECK_LT(0, watcher->channel_index());
+ CHECK_LT(static_cast<size_t>(watcher->channel_index()),
+ factory->configuration()->channels()->size());
+ results->watchers.insert(watcher->channel_index());
+ }
+ }
+ }
+
+ if (options.get_fetchers) {
+ if (report.has_fetchers()) {
+ for (const aos::timing::Fetcher *fetcher : *report.fetchers()) {
+ CHECK_LT(0, fetcher->channel_index());
+ CHECK_LT(static_cast<size_t>(fetcher->channel_index()),
+ factory->configuration()->channels()->size());
+ results->fetchers.insert(fetcher->channel_index());
+ }
+ }
+ }
+}
+
+class ChannelsExtractor {
+ public:
+ ChannelsExtractor(aos::EventLoop *node_event_loop,
+ ChannelsExtractorAccumulator *results,
+ const size_t node_index,
+ aos::SimulatedEventLoopFactory *factory,
+ const aos::Configuration *logged_configuration,
+ const aos::logger::ChannelsInLogOptions options) {
+ // skip timing report because we don't want the reader to generate a timing
+ // report
+ node_event_loop->SkipTimingReport();
+ node_event_loop->SkipAosLog();
+
+ // This is the watcher which looks for applications and then records the
+ // respective channels
+ node_event_loop->MakeWatcher(
+ "/aos", [results, node_index, factory, logged_configuration,
+ options](const aos::timing::Report &report) {
+ HandleChannelsInApplications(report, node_index, factory,
+ logged_configuration, options, results);
+ });
+ }
+};
+
+} // namespace
+namespace aos::logger {
+
+ChannelsInLogResult ChannelsInLog(
+ const std::vector<aos::logger::LogFile> &log_files,
+ const std::vector<const aos::Node *> &nodes,
+ const std::vector<std::string> &applications,
+ const aos::logger::ChannelsInLogOptions options) {
+ // Make a log_reader object to make event loop and look into channels and
+ // configuration
+ LogReader reader(log_files);
+ aos::SimulatedEventLoopFactory factory(reader.configuration());
+ reader.RegisterWithoutStarting(&factory);
+
+ ChannelsExtractorAccumulator results;
+
+ // Make watchers for every node in "nodes" and for all applications in
+ // "applications" that ran for that node
+ for (size_t ii = 0; ii < nodes.size(); ++ii) {
+ results.observed_applications.push_back({});
+ results.remaining_applications.push_back(
+ std::set<std::string>(applications.begin(), applications.end()));
+
+ const aos::Node *const node = nodes.at(ii);
+
+ aos::NodeEventLoopFactory *node_factory = factory.GetNodeEventLoopFactory(
+ aos::configuration::GetNode(factory.configuration(), node));
+
+ reader.OnStart(node,
+ [node_factory, &results, ii, &factory, &reader, &options]() {
+ node_factory->AlwaysStart<ChannelsExtractor>(
+ "channels_extractor", &results, ii, &factory,
+ reader.logged_configuration(), options);
+ });
+ }
+
+ factory.Run();
+ reader.Deregister();
+
+ for (size_t ii = 0; ii < nodes.size(); ++ii) {
+ if (!results.remaining_applications[ii].empty()) {
+ LOG(INFO) << "Didn't find all applications requested on "
+ << nodes[ii]->name()->string_view()
+ << ": remaining applications: "
+ << absl::StrJoin(results.remaining_applications[ii], ", ");
+ }
+ }
+
+ ChannelsInLogResult channels;
+
+ if (options.get_senders) {
+ channels.senders = std::make_optional<std::vector<aos::ChannelT>>({});
+ for (const int index : results.senders) {
+ channels.senders.value().push_back({});
+ reader.logged_configuration()->channels()->Get(index)->UnPackTo(
+ &channels.senders.value().back());
+ }
+ }
+
+ if (options.get_watchers) {
+ channels.watchers = std::make_optional<std::vector<aos::ChannelT>>({});
+ for (const int index : results.watchers) {
+ channels.watchers.value().push_back({});
+ reader.configuration()->channels()->Get(index)->UnPackTo(
+ &channels.watchers.value().back());
+ }
+ }
+
+ if (options.get_fetchers) {
+ channels.fetchers = std::make_optional<std::vector<aos::ChannelT>>({});
+ for (const int index : results.fetchers) {
+ channels.fetchers.value().push_back({});
+ reader.configuration()->channels()->Get(index)->UnPackTo(
+ &channels.fetchers.value().back());
+ }
+ }
+
+ if (options.get_senders && options.get_watchers && options.get_fetchers) {
+ channels.watchers_and_fetchers_without_senders =
+ std::make_optional<std::vector<aos::ChannelT>>({});
+ std::set<int> watchers_and_fetchers_without_senders;
+ // TODO(EricS) probably a better way to optimize this symmetric diff algo
+ for (const int watcher : results.watchers) {
+ if (!std::binary_search(results.senders.begin(), results.senders.end(),
+ watcher)) {
+ watchers_and_fetchers_without_senders.insert(watcher);
+ }
+ }
+
+ for (const int fetcher : results.fetchers) {
+ if (!std::binary_search(results.senders.begin(), results.senders.end(),
+ fetcher)) {
+ watchers_and_fetchers_without_senders.insert(fetcher);
+ }
+ }
+
+ for (const int index : watchers_and_fetchers_without_senders) {
+ channels.watchers_and_fetchers_without_senders.value().push_back({});
+ reader.configuration()->channels()->Get(index)->UnPackTo(
+ &channels.watchers_and_fetchers_without_senders.value().back());
+ }
+ }
+
+ return channels;
+}
+
+std::vector<aos::ChannelT> SenderChannelsInLog(
+ const std::vector<aos::logger::LogFile> &log_files,
+ const std::vector<const aos::Node *> &nodes,
+ const std::vector<std::string> &applications) {
+ return ChannelsInLog(log_files, nodes, applications, {true, false, false})
+ .senders.value();
+}
+
+std::vector<aos::ChannelT> WatcherChannelsInLog(
+ const std::vector<aos::logger::LogFile> &log_files,
+ const std::vector<const aos::Node *> &nodes,
+ const std::vector<std::string> &applications) {
+ return ChannelsInLog(log_files, nodes, applications, {false, true, false})
+ .watchers.value();
+}
+
+std::vector<aos::ChannelT> FetcherChannelsInLog(
+ const std::vector<aos::logger::LogFile> &log_files,
+ const std::vector<const aos::Node *> &nodes,
+ const std::vector<std::string> &applications) {
+ return ChannelsInLog(log_files, nodes, applications, {false, false, true})
+ .fetchers.value();
+}
+
+} // namespace aos::logger
diff --git a/aos/events/logging/log_reader_utils.h b/aos/events/logging/log_reader_utils.h
new file mode 100644
index 0000000..0c8ed07
--- /dev/null
+++ b/aos/events/logging/log_reader_utils.h
@@ -0,0 +1,54 @@
+#ifndef AOS_EVENTS_LOGGING_LOG_READER_UTILS_H_
+#define AOS_EVENTS_LOGGING_LOG_READER_UTILS_H_
+
+#include "aos/events/logging/log_reader.h"
+
+namespace aos::logger {
+
+// Utility struct for returning all channels segregated as senders, watchers and
+// fetchers
+struct ChannelsInLogResult {
+ std::optional<std::vector<aos::ChannelT>> senders;
+ std::optional<std::vector<aos::ChannelT>> watchers;
+ std::optional<std::vector<aos::ChannelT>> fetchers;
+ std::optional<std::vector<aos::ChannelT>>
+ watchers_and_fetchers_without_senders;
+}; // struct ChannelsInLogResult
+
+// A struct to select what kind of channels we want to extract from the log
+struct ChannelsInLogOptions {
+ bool get_senders = false;
+ bool get_watchers = false;
+ bool get_fetchers = false;
+}; // struct ChannelsInLogOptions
+
+// Reads the first ~1 second of timing reports in a logfile and generates a list
+// of all the channels sent on by the specified applications on the specified
+// nodes.
+ChannelsInLogResult ChannelsInLog(
+ const std::vector<aos::logger::LogFile> &log_files,
+ const std::vector<const aos::Node *> &nodes,
+ const std::vector<std::string> &applications,
+ const ChannelsInLogOptions options = ChannelsInLogOptions{true, true,
+ true});
+// Wrapper for channelsinlog but only for sender channels
+std::vector<aos::ChannelT> SenderChannelsInLog(
+ const std::vector<aos::logger::LogFile> &log_files,
+ const std::vector<const aos::Node *> &nodes,
+ const std::vector<std::string> &applications);
+
+// Wrapper for channelsinlog but only for watcher channels
+std::vector<aos::ChannelT> WatcherChannelsInLog(
+ const std::vector<aos::logger::LogFile> &log_files,
+ const std::vector<const aos::Node *> &nodes,
+ const std::vector<std::string> &applications);
+
+// Wrapper for channelsinlog but only for fetcher channels
+std::vector<aos::ChannelT> FetcherChannelsInLog(
+ const std::vector<aos::logger::LogFile> &log_files,
+ const std::vector<const aos::Node *> &nodes,
+ const std::vector<std::string> &applications);
+
+} // namespace aos::logger
+
+#endif // AOS_EVENTS_LOGGING_LOG_READER_UTILS_H_
diff --git a/aos/events/logging/log_replayer.cc b/aos/events/logging/log_replayer.cc
new file mode 100644
index 0000000..0f7444a
--- /dev/null
+++ b/aos/events/logging/log_replayer.cc
@@ -0,0 +1,178 @@
+#include <stdlib.h>
+
+#include <iostream>
+#include <optional>
+#include <ostream>
+#include <sstream>
+#include <string_view>
+#include <vector>
+
+#include "aos/configuration_generated.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_reader_utils.h"
+#include "aos/events/logging/log_replayer_config_generated.h"
+#include "aos/events/logging/logfile_sorting.h"
+#include "aos/events/logging/logfile_utils.h"
+#include "aos/events/logging/replay_timing_generated.h"
+#include "aos/events/logging/replay_timing_schema.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/util/file.h"
+#include "flatbuffers/flatbuffers.h"
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+DEFINE_string(config, "", "If specified, overrides logged configuration.");
+DEFINE_bool(
+ plot_timing, true,
+ "If set, generates a plot of the replay timing--namely, the errors between "
+ "when we "
+ "should've sent messages and when we actually sent replayed messages.");
+DEFINE_bool(skip_sender_channels, true,
+ "If set, skips replay of the channels applications replay on");
+DEFINE_bool(skip_replay, false,
+ "If set, skips actually running the replay. Useful for writing a "
+ "config without running replay");
+DEFINE_bool(
+ print_config, false,
+ "If set, prints the config that will be used for replay to stdout as json");
+DEFINE_string(
+ replay_config, "",
+ "Path to the configuration used for log replay which includes items such "
+ "as channels to remap, and applications to target for replay. If not set, "
+ "log_reader will run on shm event loop. ");
+DEFINE_string(merge_with_config, "",
+ "A valid json string to be merged with config. This is used to "
+ "add extra applications needed to run only for log_replayer");
+
+namespace aos::logger {
+
+int Main(int argc, char *argv[]) {
+ const std::vector<std::string> unsorted_logfiles =
+ aos::logger::FindLogs(argc, argv);
+
+ const std::vector<aos::logger::LogFile> logfiles =
+ aos::logger::SortParts(unsorted_logfiles);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ FLAGS_config.empty()
+ ? aos::FlatbufferDetachedBuffer<aos::Configuration>::Empty()
+ : aos::configuration::ReadConfig(FLAGS_config);
+
+ if (FLAGS_plot_timing) {
+ aos::logger::LogReader config_reader(logfiles);
+
+ // Go through the effort to add a ReplayTiming channel to ensure that we
+ // can capture timing information from the replay.
+ const aos::Configuration *raw_config = FLAGS_config.empty()
+ ? config_reader.configuration()
+ : &config.message();
+ aos::ChannelT channel_overrides;
+ channel_overrides.max_size = 10000;
+ channel_overrides.frequency = 10000;
+ config = aos::configuration::AddChannelToConfiguration(
+ raw_config, "/timing",
+ aos::FlatbufferSpan<reflection::Schema>(
+ aos::timing::ReplayTimingSchema()),
+ aos::configuration::GetMyNode(raw_config), channel_overrides);
+ }
+
+ if (!FLAGS_merge_with_config.empty()) {
+ config = aos::configuration::MergeWithConfig(&config.message(),
+ FLAGS_merge_with_config);
+ }
+
+ std::optional<aos::FlatbufferDetachedBuffer<ReplayConfig>> replay_config =
+ FLAGS_replay_config.empty()
+ ? std::nullopt
+ : std::make_optional(aos::JsonToFlatbuffer<ReplayConfig>(
+ aos::util::ReadFileToStringOrDie(FLAGS_replay_config.data())));
+
+ std::vector<std::pair<std::string_view, std::string_view>> message_filter;
+ if (FLAGS_skip_sender_channels && replay_config.has_value()) {
+ CHECK(replay_config.value().message().has_active_nodes());
+ std::vector<const Node *> active_nodes;
+ for (const auto &node : *replay_config.value().message().active_nodes()) {
+ active_nodes.emplace_back(configuration::GetNode(
+ &config.message(), node->name()->string_view()));
+ }
+
+ std::vector<std::string> applications;
+ for (const auto &application :
+ *replay_config.value().message().applications()) {
+ if (application->name()->string_view() != "camera_message_interceptor") {
+ applications.emplace_back(application->name()->string_view());
+ }
+ }
+
+ aos::logger::ChannelsInLogResult channels =
+ ChannelsInLog(logfiles, active_nodes, applications);
+ for (auto const &channel :
+ channels.watchers_and_fetchers_without_senders.value()) {
+ message_filter.emplace_back(std::make_pair(channel.name, channel.type));
+ }
+ }
+
+ aos::logger::LogReader reader(
+ logfiles, &config.message(),
+ message_filter.empty() ? nullptr : &message_filter);
+
+ if (replay_config.has_value()) {
+ for (auto const &remap_channel :
+ *replay_config.value().message().remap_channels()) {
+ auto const &channel = remap_channel->channel();
+ std::string_view new_type = remap_channel->has_new_type()
+ ? remap_channel->new_type()->string_view()
+ : channel->type()->string_view();
+ reader.RemapLoggedChannel(
+ channel->name()->string_view(), channel->type()->string_view(),
+ remap_channel->prefix()->string_view(), new_type);
+ }
+ }
+
+ if (FLAGS_print_config) {
+ // TODO(Naman): Replace with config writer if it will be cleaner
+ std::cout << FlatbufferToJson(reader.configuration()) << std::endl;
+ }
+
+ if (!FLAGS_skip_replay) {
+ aos::ShmEventLoop event_loop(reader.configuration());
+
+ event_loop.SkipAosLog();
+ event_loop.SkipTimingReport();
+
+ reader.Register(&event_loop);
+ reader.OnEnd(event_loop.node(), [&event_loop]() { event_loop.Exit(); });
+
+ if (FLAGS_plot_timing) {
+ aos::Sender<aos::timing::ReplayTiming> replay_timing_sender =
+ event_loop.MakeSender<aos::timing::ReplayTiming>("/timing");
+ reader.set_timing_accuracy_sender(event_loop.node(),
+ std::move(replay_timing_sender));
+ }
+
+ event_loop.Run();
+
+ reader.Deregister();
+ }
+
+ return EXIT_SUCCESS;
+}
+
+} // namespace aos::logger
+
+int main(int argc, char *argv[]) {
+ gflags::SetUsageMessage(
+ R"message(Binary to replay the full contents of a logfile into shared memory.
+ #replay_config should be set in order to replay a set of nodes, applications and channels
+ #print config and skip replay, if you only want to print the config and not do log replay
+ Use case #1: log_replayer <log_dir> --print_config --replay_config=<path_to_config> --skip_replay
+ Use case #2: log_replayer <log_dir> --nofatal_sent_too_fast --replay_config=<path_to_config>
+ )message");
+
+ aos::InitGoogle(&argc, &argv);
+ return aos::logger::Main(argc, argv);
+}
diff --git a/aos/events/logging/log_replayer_config.fbs b/aos/events/logging/log_replayer_config.fbs
new file mode 100644
index 0000000..13a3cd9
--- /dev/null
+++ b/aos/events/logging/log_replayer_config.fbs
@@ -0,0 +1,29 @@
+include "../../configuration.fbs";
+
+namespace aos;
+
+// A flatbuffer table to store the RemapChannel used inside ReplayConfig
+table RemapChannel {
+ // table Channel defined in configuration.fbs
+ // Specify the old channel's name, type and other contents of the table
+ channel:Channel(id : 0);
+ // A prefix used for remapping the channel from /channel_name to
+ // /prefix/channel_name
+ prefix:string(id : 1);
+ // new type for the remapped channel
+ new_type:string(id : 2);
+}
+
+// A flatbuffer message to store the replay config
+// This is used to replay a log with specific applications, active nodes and
+// remaps the channels if needed
+table ReplayConfig {
+ // applications on which log needs to be replayed
+ applications:[Application](id : 0);
+ // Specify the nodes which need to be replayed
+ active_nodes:[Node](id : 1);
+// Specify the channels in the config which need to be remapped
+ remap_channels:[RemapChannel](id : 2);
+}
+
+root_type ReplayConfig;
diff --git a/aos/events/logging/log_writer.cc b/aos/events/logging/log_writer.cc
index ab7d3c6..5671a3f 100644
--- a/aos/events/logging/log_writer.cc
+++ b/aos/events/logging/log_writer.cc
@@ -36,9 +36,9 @@
timer_handler_->set_name("channel_poll");
VLOG(1) << "Creating logger for " << FlatbufferToJson(node_);
- // When we are logging remote timestamps, we need to be able to translate from
- // the channel index that the event loop uses to the channel index in the
- // config in the log file.
+ // When we are logging remote timestamps, we need to be able to translate
+ // from the channel index that the event loop uses to the channel index in
+ // the config in the log file.
event_loop_to_logged_channel_index_.resize(
event_loop->configuration()->channels()->size(), -1);
for (size_t event_loop_channel_index = 0;
@@ -231,14 +231,20 @@
}
bool Logger::RenameLogBase(std::string new_base_name) {
- if (new_base_name == log_namer_->base_name()) {
+ // TODO(Naman): Got a crash in RenameLogBase. Putting in a CHECK_NOTNULL to
+ // catch the bug if it happens again
+ if (new_base_name == CHECK_NOTNULL(log_namer_)->base_name()) {
return true;
}
std::string current_directory = std::string(log_namer_->base_name());
std::string new_directory = new_base_name;
auto current_path_split = current_directory.rfind("/");
+ CHECK(current_path_split != std::string::npos)
+ << "Could not find / in the current directory path";
auto new_path_split = new_directory.rfind("/");
+ CHECK(new_path_split != std::string::npos)
+ << "Could not find / in the new directory path";
CHECK(new_base_name.substr(new_path_split) ==
current_directory.substr(current_path_split))
@@ -365,9 +371,9 @@
// worse.
// TODO(austin): Test...
//
- // This is safe to call here since we have set last_synchronized_time_ as the
- // same time as in the header, and all the data before it should be logged
- // without ordering concerns.
+ // This is safe to call here since we have set last_synchronized_time_ as
+ // the same time as in the header, and all the data before it should be
+ // logged without ordering concerns.
LogUntil(last_synchronized_time_);
timer_handler_->Setup(event_loop_->monotonic_now() + polling_period_,
@@ -389,8 +395,8 @@
std::unique_ptr<LogNamer> old_log_namer = std::move(log_namer_);
log_namer_ = std::move(log_namer);
- // Now grab a representative time on both the RT and monotonic clock. Average
- // a monotonic clock before and after to reduce the error.
+ // Now grab a representative time on both the RT and monotonic clock.
+ // Average a monotonic clock before and after to reduce the error.
const aos::monotonic_clock::time_point beginning_time =
event_loop_->monotonic_now();
const aos::realtime_clock::time_point beginning_time_rt =
@@ -403,11 +409,11 @@
<< "ns to swap log_namer";
}
- // Since we are going to log all in 1 big go, we need our log start time to be
- // after the previous LogUntil call finished, but before 1 period after it.
- // The best way to guarentee that is to pick a start time that is the earliest
- // of the two. That covers the case where the OS puts us to sleep between
- // when we finish LogUntil and capture beginning_time.
+ // Since we are going to log all in 1 big go, we need our log start time to
+ // be after the previous LogUntil call finished, but before 1 period after
+ // it. The best way to guarentee that is to pick a start time that is the
+ // earliest of the two. That covers the case where the OS puts us to sleep
+ // between when we finish LogUntil and capture beginning_time.
const aos::monotonic_clock::time_point monotonic_start_time =
std::min(last_synchronized_time_, beginning_time);
const aos::realtime_clock::time_point realtime_start_time =
@@ -426,8 +432,9 @@
// Note that WriteHeader updates last_synchronized_time_ to be the
// current time when it is called, which is then the "start time"
// of the new (restarted) log. This timestamp will be after
- // the timestamp of the last message fetched on each channel, but is carefully
- // picked per the comment above to not violate max_out_of_order_duration.
+ // the timestamp of the last message fetched on each channel, but is
+ // carefully picked per the comment above to not violate
+ // max_out_of_order_duration.
WriteHeader(monotonic_start_time, realtime_start_time);
const aos::monotonic_clock::time_point header_time =
@@ -762,8 +769,8 @@
// Class to copy a context into the provided buffer.
class ContextDataCopier : public DataEncoder::Copier {
public:
- ContextDataCopier(const Context &context, int channel_index,
- LogType log_type, EventLoop *event_loop)
+ ContextDataCopier(const Context &context, int channel_index, LogType log_type,
+ EventLoop *event_loop)
: DataEncoder::Copier(PackMessageSize(log_type, context.size)),
context_(context),
channel_index_(channel_index),
@@ -790,10 +797,10 @@
// Class to copy a RemoteMessage into the provided buffer.
class RemoteMessageCopier : public DataEncoder::Copier {
public:
- RemoteMessageCopier(
- const message_bridge::RemoteMessage *message, int channel_index,
- aos::monotonic_clock::time_point monotonic_timestamp_time,
- EventLoop *event_loop)
+ RemoteMessageCopier(const message_bridge::RemoteMessage *message,
+ int channel_index,
+ aos::monotonic_clock::time_point monotonic_timestamp_time,
+ EventLoop *event_loop)
: DataEncoder::Copier(PackRemoteMessageSize()),
message_(message),
channel_index_(channel_index),
@@ -826,8 +833,8 @@
// Write!
const auto start = event_loop_->monotonic_now();
- ContextDataCopier coppier(f.fetcher->context(), f.channel_index,
- f.log_type, event_loop_);
+ ContextDataCopier coppier(f.fetcher->context(), f.channel_index, f.log_type,
+ event_loop_);
writer->CopyMessage(&coppier, source_node_boot_uuid, start);
RecordCreateMessageTime(start, coppier.end_time(), f);
@@ -852,7 +859,7 @@
const auto start = event_loop_->monotonic_now();
ContextDataCopier coppier(f.fetcher->context(), f.channel_index,
- LogType::kLogDeliveryTimeOnly, event_loop_);
+ LogType::kLogDeliveryTimeOnly, event_loop_);
timestamp_writer->CopyMessage(&coppier, event_loop_->boot_uuid(), start);
RecordCreateMessageTime(start, coppier.end_time(), f);
@@ -902,7 +909,7 @@
reliable, monotonic_timestamp_time);
RemoteMessageCopier coppier(msg, channel_index, monotonic_timestamp_time,
- event_loop_);
+ event_loop_);
contents_writer->CopyMessage(&coppier, UUID::FromVector(msg->boot_uuid()),
start);
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index 94337d3..90220c8 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -129,27 +129,12 @@
return *this;
}
-void DetachedBufferWriter::QueueSpan(absl::Span<const uint8_t> span) {
+void DetachedBufferWriter::CopyMessage(DataEncoder::Copier *coppier,
+ aos::monotonic_clock::time_point now) {
if (ran_out_of_space_) {
// We don't want any later data to be written after space becomes
// available, so refuse to write anything more once we've dropped data
// because we ran out of space.
- VLOG(1) << "Ignoring span: " << span.size();
- return;
- }
-
- if (!encoder_->HasSpace(span.size())) {
- Flush();
- CHECK(encoder_->HasSpace(span.size()));
- }
- DataEncoder::SpanCopier coppier(span);
- encoder_->Encode(&coppier);
- FlushAtThreshold(aos::monotonic_clock::now());
-}
-
-void DetachedBufferWriter::CopyMessage(DataEncoder::Copier *coppier,
- aos::monotonic_clock::time_point now) {
- if (ran_out_of_space_) {
return;
}
@@ -1804,12 +1789,30 @@
}
bool TimestampMapper::QueueMatched() {
+ MatchResult result = MatchResult::kEndOfFile;
+ do {
+ result = MaybeQueueMatched();
+ } while (result == MatchResult::kSkipped);
+ return result == MatchResult::kQueued;
+}
+
+bool TimestampMapper::CheckReplayChannelsAndMaybePop(
+ const TimestampedMessage & /*message*/) {
+ if (replay_channels_callback_ &&
+ !replay_channels_callback_(matched_messages_.back())) {
+ matched_messages_.pop_back();
+ return true;
+ }
+ return false;
+}
+
+TimestampMapper::MatchResult TimestampMapper::MaybeQueueMatched() {
if (nodes_data_.empty()) {
// Simple path. We are single node, so there are no timestamps to match!
CHECK_EQ(messages_.size(), 0u);
Message *m = boot_merger_.Front();
if (!m) {
- return false;
+ return MatchResult::kEndOfFile;
}
// Enqueue this message into matched_messages_ so we have a place to
// associate remote timestamps, and return it.
@@ -1821,7 +1824,10 @@
// We are thin wrapper around node_merger. Call it directly.
boot_merger_.PopFront();
timestamp_callback_(&matched_messages_.back());
- return true;
+ if (CheckReplayChannelsAndMaybePop(matched_messages_.back())) {
+ return MatchResult::kSkipped;
+ }
+ return MatchResult::kQueued;
}
// We need to only add messages to the list so they get processed for
@@ -1830,7 +1836,7 @@
if (messages_.empty()) {
if (!Queue()) {
// Found nothing to add, we are out of data!
- return false;
+ return MatchResult::kEndOfFile;
}
// Now that it has been added (and cannibalized), forget about it
@@ -1847,7 +1853,10 @@
last_message_time_ = matched_messages_.back().monotonic_event_time;
messages_.pop_front();
timestamp_callback_(&matched_messages_.back());
- return true;
+ if (CheckReplayChannelsAndMaybePop(matched_messages_.back())) {
+ return MatchResult::kSkipped;
+ }
+ return MatchResult::kQueued;
} else {
// Got a timestamp, find the matching remote data, match it, and return
// it.
@@ -1874,7 +1883,10 @@
// Since messages_ holds the data, drop it.
messages_.pop_front();
timestamp_callback_(&matched_messages_.back());
- return true;
+ if (CheckReplayChannelsAndMaybePop(matched_messages_.back())) {
+ return MatchResult::kSkipped;
+ }
+ return MatchResult::kQueued;
}
}
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 7105b0c..ca36ace 100644
--- a/aos/events/logging/logfile_utils.h
+++ b/aos/events/logging/logfile_utils.h
@@ -76,9 +76,6 @@
void CopyMessage(DataEncoder::Copier *coppier,
aos::monotonic_clock::time_point now);
- // Queues up data in span. May copy or may write it to disk immediately.
- void QueueSpan(absl::Span<const uint8_t> span);
-
// Indicates we got ENOSPC when trying to write. After this returns true, no
// further data is written.
bool ran_out_of_space() const { return ran_out_of_space_; }
@@ -787,12 +784,25 @@
}
}
+ // Sets the callback that can be used to skip messages.
+ void set_replay_channels_callback(
+ std::function<bool(const TimestampedMessage &)> fn) {
+ replay_channels_callback_ = fn;
+ }
+
// Sets a callback to be called whenever a full message is queued.
void set_timestamp_callback(std::function<void(TimestampedMessage *)> fn) {
timestamp_callback_ = fn;
}
private:
+ // Result of MaybeQueueMatched
+ enum class MatchResult : uint8_t {
+ kEndOfFile, // End of the log file being read
+ kQueued, // Message was queued
+ kSkipped // Message was skipped over
+ };
+
// The state for a remote node. This holds the data that needs to be matched
// with the remote node's timestamps.
struct NodeData {
@@ -835,6 +845,10 @@
// true if one was queued, and false otherwise.
bool QueueMatched();
+ // Queues a message if the replay_channels_callback is passed and the end of
+ // the log file has not been reached.
+ MatchResult MaybeQueueMatched();
+
// Queues up data until we have at least one message >= to time t.
// Useful for triggering a remote node to read enough data to have the
// timestamp you care about available.
@@ -843,6 +857,11 @@
// Queues m into matched_messages_.
void QueueMessage(Message *m);
+ // If a replay_channels_callback was set and the callback returns false, a
+ // matched message is popped and true is returned. Otherwise false is
+ // returned.
+ bool CheckReplayChannelsAndMaybePop(const TimestampedMessage &message);
+
// Returns the name of the node this class is sorting for.
std::string_view node_name() const {
return configuration_->has_nodes() ? configuration_->nodes()
@@ -886,6 +905,7 @@
BootTimestamp queued_until_ = BootTimestamp::min_time();
std::function<void(TimestampedMessage *)> timestamp_callback_;
+ std::function<bool(TimestampedMessage &)> replay_channels_callback_;
};
// Returns the node name with a trailing space, or an empty string if we are on
diff --git a/aos/events/logging/logfile_utils_out_of_space_test_runner.cc b/aos/events/logging/logfile_utils_out_of_space_test_runner.cc
index afc4d77..719eb6e 100644
--- a/aos/events/logging/logfile_utils_out_of_space_test_runner.cc
+++ b/aos/events/logging/logfile_utils_out_of_space_test_runner.cc
@@ -22,10 +22,14 @@
FLAGS_tmpfs + "/file",
std::make_unique<aos::logger::DummyEncoder>(data.size()));
for (int i = 0; i < 8; ++i) {
- writer.QueueSpan(data);
+ aos::logger::DataEncoder::SpanCopier coppier(data);
+ writer.CopyMessage(&coppier, aos::monotonic_clock::now());
CHECK(!writer.ran_out_of_space()) << ": " << i;
}
- writer.QueueSpan(data);
+ {
+ aos::logger::DataEncoder::SpanCopier coppier(data);
+ writer.CopyMessage(&coppier, aos::monotonic_clock::now());
+ }
CHECK(writer.ran_out_of_space());
writer.acknowledge_out_of_space();
}
diff --git a/aos/events/logging/logfile_utils_test.cc b/aos/events/logging/logfile_utils_test.cc
index 3d99757..b3a9bbd 100644
--- a/aos/events/logging/logfile_utils_test.cc
+++ b/aos/events/logging/logfile_utils_test.cc
@@ -24,8 +24,8 @@
namespace logger {
namespace testing {
namespace chrono = std::chrono;
-using aos::testing::ArtifactPath;
using aos::message_bridge::RemoteMessage;
+using aos::testing::ArtifactPath;
// Adapter class to make it easy to test DetachedBufferWriter without adding
// test only boilerplate to DetachedBufferWriter.
@@ -39,6 +39,10 @@
void WriteSizedFlatbuffer(flatbuffers::DetachedBuffer &&buffer) {
QueueSpan(absl::Span<const uint8_t>(buffer.data(), buffer.size()));
}
+ void QueueSpan(absl::Span<const uint8_t> buffer) {
+ DataEncoder::SpanCopier coppier(buffer);
+ CopyMessage(&coppier, monotonic_clock::now());
+ }
};
// Creates a size prefixed flatbuffer from json.
@@ -1036,6 +1040,143 @@
}
}
+// Tests that we filter messages using the channel filter callback
+TEST_F(TimestampMapperTest, ReplayChannelsCallbackTest) {
+ const aos::monotonic_clock::time_point e = monotonic_clock::epoch();
+ {
+ TestDetachedBufferWriter writer0(logfile0_);
+ writer0.QueueSpan(config0_.span());
+ TestDetachedBufferWriter writer1(logfile1_);
+ writer1.QueueSpan(config2_.span());
+
+ writer0.WriteSizedFlatbuffer(
+ MakeLogMessage(e + chrono::milliseconds(1000), 0, 0x005));
+ writer1.WriteSizedFlatbuffer(MakeTimestampMessage(
+ e + chrono::milliseconds(1000), 0, chrono::seconds(100)));
+
+ writer0.WriteSizedFlatbuffer(
+ MakeLogMessage(e + chrono::milliseconds(2000), 0, 0x006));
+ writer1.WriteSizedFlatbuffer(MakeTimestampMessage(
+ e + chrono::milliseconds(2000), 0, chrono::seconds(100)));
+
+ writer0.WriteSizedFlatbuffer(
+ MakeLogMessage(e + chrono::milliseconds(3000), 0, 0x007));
+ writer1.WriteSizedFlatbuffer(MakeTimestampMessage(
+ e + chrono::milliseconds(3000), 0, chrono::seconds(100)));
+ }
+
+ const std::vector<LogFile> parts = SortParts({logfile0_, logfile1_});
+
+ ASSERT_EQ(parts[0].logger_node, "pi1");
+ ASSERT_EQ(parts[1].logger_node, "pi2");
+
+ // mapper0 will not provide any messages while mapper1 will provide all
+ // messages due to the channel filter callbacks used
+ size_t mapper0_count = 0;
+ TimestampMapper mapper0(FilterPartsForNode(parts, "pi1"));
+ mapper0.set_timestamp_callback(
+ [&](TimestampedMessage *) { ++mapper0_count; });
+ mapper0.set_replay_channels_callback(
+ [&](const TimestampedMessage &) -> bool { return mapper0_count != 2; });
+ size_t mapper1_count = 0;
+ TimestampMapper mapper1(FilterPartsForNode(parts, "pi2"));
+ mapper1.set_timestamp_callback(
+ [&](TimestampedMessage *) { ++mapper1_count; });
+ mapper1.set_replay_channels_callback(
+ [&](const TimestampedMessage &) -> bool { return mapper1_count != 2; });
+
+ mapper0.AddPeer(&mapper1);
+ mapper1.AddPeer(&mapper0);
+
+ {
+ std::deque<TimestampedMessage> output0;
+
+ EXPECT_EQ(mapper0_count, 0u);
+ EXPECT_EQ(mapper1_count, 0u);
+
+ ASSERT_TRUE(mapper0.Front() != nullptr);
+ EXPECT_EQ(mapper0_count, 1u);
+ EXPECT_EQ(mapper1_count, 0u);
+ output0.emplace_back(std::move(*mapper0.Front()));
+ mapper0.PopFront();
+
+ EXPECT_TRUE(mapper0.started());
+ EXPECT_EQ(mapper0_count, 1u);
+ EXPECT_EQ(mapper1_count, 0u);
+
+ // mapper0_count is now at 3 since the second message is not queued, but
+ // timestamp_callback needs to be called everytime even if Front() does not
+ // provide a message due to the replay_channels_callback.
+ ASSERT_TRUE(mapper0.Front() != nullptr);
+ EXPECT_EQ(mapper0_count, 3u);
+ EXPECT_EQ(mapper1_count, 0u);
+ output0.emplace_back(std::move(*mapper0.Front()));
+ mapper0.PopFront();
+
+ EXPECT_TRUE(mapper0.started());
+ EXPECT_EQ(mapper0_count, 3u);
+ EXPECT_EQ(mapper1_count, 0u);
+
+ ASSERT_TRUE(mapper0.Front() == nullptr);
+ EXPECT_TRUE(mapper0.started());
+
+ EXPECT_EQ(mapper0_count, 3u);
+ EXPECT_EQ(mapper1_count, 0u);
+
+ EXPECT_EQ(output0[0].monotonic_event_time.boot, 0u);
+ EXPECT_EQ(output0[0].monotonic_event_time.time,
+ e + chrono::milliseconds(1000));
+ EXPECT_TRUE(output0[0].data != nullptr);
+
+ EXPECT_EQ(output0[1].monotonic_event_time.boot, 0u);
+ EXPECT_EQ(output0[1].monotonic_event_time.time,
+ e + chrono::milliseconds(3000));
+ EXPECT_TRUE(output0[1].data != nullptr);
+ }
+
+ {
+ SCOPED_TRACE("Trying node1 now");
+ std::deque<TimestampedMessage> output1;
+
+ EXPECT_EQ(mapper0_count, 3u);
+ EXPECT_EQ(mapper1_count, 0u);
+
+ ASSERT_TRUE(mapper1.Front() != nullptr);
+ EXPECT_EQ(mapper0_count, 3u);
+ EXPECT_EQ(mapper1_count, 1u);
+ output1.emplace_back(std::move(*mapper1.Front()));
+ mapper1.PopFront();
+ EXPECT_TRUE(mapper1.started());
+ EXPECT_EQ(mapper0_count, 3u);
+ EXPECT_EQ(mapper1_count, 1u);
+
+ // mapper1_count is now at 3 since the second message is not queued, but
+ // timestamp_callback needs to be called everytime even if Front() does not
+ // provide a message due to the replay_channels_callback.
+ ASSERT_TRUE(mapper1.Front() != nullptr);
+ output1.emplace_back(std::move(*mapper1.Front()));
+ mapper1.PopFront();
+ EXPECT_TRUE(mapper1.started());
+
+ EXPECT_EQ(mapper0_count, 3u);
+ EXPECT_EQ(mapper1_count, 3u);
+
+ ASSERT_TRUE(mapper1.Front() == nullptr);
+
+ EXPECT_EQ(mapper0_count, 3u);
+ EXPECT_EQ(mapper1_count, 3u);
+
+ EXPECT_EQ(output1[0].monotonic_event_time.boot, 0u);
+ EXPECT_EQ(output1[0].monotonic_event_time.time,
+ e + chrono::seconds(100) + chrono::milliseconds(1000));
+ EXPECT_TRUE(output1[0].data != nullptr);
+
+ EXPECT_EQ(output1[1].monotonic_event_time.boot, 0u);
+ EXPECT_EQ(output1[1].monotonic_event_time.time,
+ e + chrono::seconds(100) + chrono::milliseconds(3000));
+ EXPECT_TRUE(output1[1].data != nullptr);
+ }
+}
// Tests that a MessageHeader with monotonic_timestamp_time set gets properly
// returned.
TEST_F(TimestampMapperTest, MessageWithTimestampTime) {
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index c99a7c8..18c8aa1 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -3708,7 +3708,6 @@
event_loop_factory_.RunFor(chrono::milliseconds(95));
-
StartLogger(&pi1_logger);
aos::monotonic_clock::time_point last_rotation_time =
pi1_logger.event_loop->monotonic_now();
diff --git a/aos/events/logging/realtime_replay_test.cc b/aos/events/logging/realtime_replay_test.cc
index 0cdf9fb..888b043 100644
--- a/aos/events/logging/realtime_replay_test.cc
+++ b/aos/events/logging/realtime_replay_test.cc
@@ -1,12 +1,15 @@
#include "aos/events/logging/log_reader.h"
#include "aos/events/logging/log_writer.h"
#include "aos/events/ping_lib.h"
+#include "aos/events/pong_lib.h"
#include "aos/events/shm_event_loop.h"
#include "aos/json_to_flatbuffer.h"
#include "aos/testing/path.h"
#include "aos/testing/tmpdir.h"
#include "gtest/gtest.h"
+DECLARE_string(override_hostname);
+
namespace aos::logger::testing {
class RealtimeLoggerTest : public ::testing::Test {
@@ -18,12 +21,54 @@
config_(aos::configuration::ReadConfig(config_file_)),
event_loop_factory_(&config_.message()),
ping_event_loop_(event_loop_factory_.MakeEventLoop("ping")),
- ping_(ping_event_loop_.get()) {
+ pong_event_loop_(event_loop_factory_.MakeEventLoop("pong")),
+ ping_(ping_event_loop_.get()),
+ pong_(pong_event_loop_.get()),
+ tmpdir_(aos::testing::TestTmpDir()),
+ base_name_(tmpdir_ + "/logfile/") {
FLAGS_shm_base = shm_dir_;
- // Nuke the shm dir, to ensure we aren't being affected by any preexisting
- // tests.
+ // Nuke the shm and log dirs, to ensure we aren't being affected by any
+ // preexisting tests.
aos::util::UnlinkRecursive(shm_dir_);
+ aos::util::UnlinkRecursive(base_name_);
+ }
+
+ gflags::FlagSaver flag_saver_;
+ std::string shm_dir_;
+
+ const std::string config_file_;
+ const aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+
+ // Factory and Ping class to generate a test logfile.
+ SimulatedEventLoopFactory event_loop_factory_;
+ std::unique_ptr<EventLoop> ping_event_loop_;
+ std::unique_ptr<EventLoop> pong_event_loop_;
+ Ping ping_;
+ Pong pong_;
+ const std::string tmpdir_;
+ const std::string base_name_;
+};
+
+class RealtimeMultiNodeLoggerTest : public ::testing::Test {
+ protected:
+ RealtimeMultiNodeLoggerTest()
+ : shm_dir_(aos::testing::TestTmpDir() + "/aos"),
+ config_file_(aos::testing::ArtifactPath(
+ "aos/events/logging/multinode_pingpong_combined_config.json")),
+ config_(aos::configuration::ReadConfig(config_file_)),
+ event_loop_factory_(&config_.message()),
+ ping_event_loop_(event_loop_factory_.MakeEventLoop(
+ "pi1", configuration::GetNode(&config_.message(), "pi1"))),
+ ping_(ping_event_loop_.get()),
+ tmpdir_(aos::testing::TestTmpDir()),
+ base_name_(tmpdir_ + "/logfile/") {
+ FLAGS_shm_base = shm_dir_;
+
+ // Nuke the shm and log dirs, to ensure we aren't being affected by any
+ // preexisting tests.
+ aos::util::UnlinkRecursive(shm_dir_);
+ aos::util::UnlinkRecursive(base_name_);
}
gflags::FlagSaver flag_saver_;
@@ -36,12 +81,11 @@
SimulatedEventLoopFactory event_loop_factory_;
std::unique_ptr<EventLoop> ping_event_loop_;
Ping ping_;
+ const std::string tmpdir_;
+ const std::string base_name_;
};
TEST_F(RealtimeLoggerTest, RealtimeReplay) {
- const std::string tmpdir = aos::testing::TestTmpDir();
- const std::string base_name = tmpdir + "/logfile/";
- aos::util::UnlinkRecursive(base_name);
{
std::unique_ptr<EventLoop> logger_event_loop =
event_loop_factory_.MakeEventLoop("logger");
@@ -51,11 +95,11 @@
Logger logger(logger_event_loop.get());
logger.set_separate_config(false);
logger.set_polling_period(std::chrono::milliseconds(100));
- logger.StartLoggingOnRun(base_name);
+ logger.StartLoggingOnRun(base_name_);
event_loop_factory_.RunFor(std::chrono::milliseconds(2000));
}
- LogReader reader(logger::SortParts(logger::FindLogs(base_name)));
+ LogReader reader(logger::SortParts(logger::FindLogs(base_name_)));
ShmEventLoop shm_event_loop(reader.configuration());
reader.Register(&shm_event_loop);
reader.OnEnd(shm_event_loop.node(),
@@ -73,4 +117,248 @@
ASSERT_TRUE(ping_fetcher.Fetch());
ASSERT_EQ(ping_fetcher->value(), 210);
}
+
+// Tests that ReplayChannels causes no messages to be replayed other than what
+// is included on a single node config
+TEST_F(RealtimeLoggerTest, SingleNodeReplayChannels) {
+ {
+ std::unique_ptr<EventLoop> logger_event_loop =
+ event_loop_factory_.MakeEventLoop("logger");
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(95));
+
+ Logger logger(logger_event_loop.get());
+ logger.set_separate_config(false);
+ logger.set_polling_period(std::chrono::milliseconds(100));
+ logger.StartLoggingOnRun(base_name_);
+ event_loop_factory_.RunFor(std::chrono::milliseconds(2000));
+ }
+
+ ReplayChannels replay_channels{{"/test", "aos.examples.Ping"}};
+ LogReader reader(logger::SortParts(logger::FindLogs(base_name_)),
+ &config_.message(), &replay_channels);
+ ShmEventLoop shm_event_loop(reader.configuration());
+ reader.Register(&shm_event_loop);
+ reader.OnEnd(shm_event_loop.node(),
+ [&shm_event_loop]() { shm_event_loop.Exit(); });
+
+ Fetcher<examples::Ping> ping_fetcher =
+ shm_event_loop.MakeFetcher<examples::Ping>("/test");
+ Fetcher<examples::Pong> pong_fetcher =
+ shm_event_loop.MakeFetcher<examples::Pong>("/test");
+
+ shm_event_loop.AddTimer([]() { LOG(INFO) << "Hello, World!"; })
+ ->Setup(shm_event_loop.monotonic_now(), std::chrono::seconds(1));
+
+ auto *const end_timer = shm_event_loop.AddTimer([&shm_event_loop]() {
+ LOG(INFO) << "All done, quitting now";
+ shm_event_loop.Exit();
+ });
+
+ // TODO(EricS) reader.OnEnd() is not working as expected when
+ // using a channel filter.
+ // keep looking for 3 seconds if some message comes, just in case
+ size_t run_seconds = 3;
+ shm_event_loop.OnRun([&shm_event_loop, end_timer, run_seconds]() {
+ LOG(INFO) << "Quitting in: " << run_seconds;
+ end_timer->Setup(shm_event_loop.monotonic_now() +
+ std::chrono::seconds(run_seconds));
+ });
+ shm_event_loop.Run();
+ reader.Deregister();
+
+ ASSERT_TRUE(ping_fetcher.Fetch());
+ ASSERT_EQ(ping_fetcher->value(), 210);
+ ASSERT_FALSE(pong_fetcher.Fetch());
+}
+
+// Tests that ReplayChannels causes no messages to be replayed other than what
+// is included on a multi node config
+TEST_F(RealtimeMultiNodeLoggerTest, ReplayChannelsPingTest) {
+ FLAGS_override_hostname = "raspberrypi";
+ {
+ std::unique_ptr<EventLoop> logger_event_loop =
+ event_loop_factory_.MakeEventLoop(
+ "logger", configuration::GetNode(&config_.message(), "pi1"));
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(95));
+
+ Logger logger(logger_event_loop.get());
+ logger.set_separate_config(false);
+ logger.set_polling_period(std::chrono::milliseconds(100));
+
+ std::unique_ptr<MultiNodeLogNamer> namer =
+ std::make_unique<MultiNodeLogNamer>(
+ base_name_, &config_.message(), logger_event_loop.get(),
+ configuration::GetNode(&config_.message(), "pi1"));
+
+ logger.StartLogging(std::move(namer));
+ event_loop_factory_.RunFor(std::chrono::milliseconds(2000));
+ }
+
+ ReplayChannels replay_channels{{"/test", "aos.examples.Ping"}};
+ LogReader reader(logger::SortParts(logger::FindLogs(base_name_)),
+ &config_.message(), &replay_channels);
+ ShmEventLoop shm_event_loop(reader.configuration());
+ reader.Register(&shm_event_loop);
+ reader.OnEnd(shm_event_loop.node(),
+ [&shm_event_loop]() { shm_event_loop.Exit(); });
+
+ Fetcher<examples::Ping> ping_fetcher =
+ shm_event_loop.MakeFetcher<examples::Ping>("/test");
+
+ shm_event_loop.AddTimer([]() { LOG(INFO) << "Hello, World!"; })
+ ->Setup(shm_event_loop.monotonic_now(), std::chrono::seconds(1));
+
+ shm_event_loop.Run();
+ reader.Deregister();
+
+ ASSERT_TRUE(ping_fetcher.Fetch());
+ ASSERT_EQ(ping_fetcher->value(), 210);
+}
+
+// Tests that when remapping a channel included in ReplayChannels messages are
+// sent on the remapped channel
+TEST_F(RealtimeMultiNodeLoggerTest, RemappedReplayChannelsTest) {
+ FLAGS_override_hostname = "raspberrypi";
+ {
+ std::unique_ptr<EventLoop> logger_event_loop =
+ event_loop_factory_.MakeEventLoop(
+ "logger", configuration::GetNode(&config_.message(), "pi1"));
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(95));
+
+ Logger logger(logger_event_loop.get());
+ logger.set_separate_config(false);
+ logger.set_polling_period(std::chrono::milliseconds(100));
+
+ std::unique_ptr<MultiNodeLogNamer> namer =
+ std::make_unique<MultiNodeLogNamer>(
+ base_name_, &config_.message(), logger_event_loop.get(),
+ configuration::GetNode(&config_.message(), "pi1"));
+
+ logger.StartLogging(std::move(namer));
+ event_loop_factory_.RunFor(std::chrono::milliseconds(2000));
+ }
+
+ ReplayChannels replay_channels{{"/test", "aos.examples.Ping"}};
+ LogReader reader(logger::SortParts(logger::FindLogs(base_name_)),
+ &config_.message(), &replay_channels);
+ reader.RemapLoggedChannel<aos::examples::Ping>("/test", "/original");
+ ShmEventLoop shm_event_loop(reader.configuration());
+ reader.Register(&shm_event_loop);
+ reader.OnEnd(shm_event_loop.node(),
+ [&shm_event_loop]() { shm_event_loop.Exit(); });
+
+ Fetcher<examples::Ping> original_ping_fetcher =
+ shm_event_loop.MakeFetcher<examples::Ping>("/original/test");
+
+ Fetcher<examples::Ping> ping_fetcher =
+ shm_event_loop.MakeFetcher<examples::Ping>("/test");
+
+ shm_event_loop.AddTimer([]() { LOG(INFO) << "Hello, World!"; })
+ ->Setup(shm_event_loop.monotonic_now(), std::chrono::seconds(1));
+
+ shm_event_loop.Run();
+ reader.Deregister();
+
+ ASSERT_TRUE(original_ping_fetcher.Fetch());
+ ASSERT_EQ(original_ping_fetcher->value(), 210);
+ ASSERT_FALSE(ping_fetcher.Fetch());
+}
+
+// Tests that messages are not replayed when they do not exist in the
+// ReplayChannels provided to LogReader. The channels used here do not
+// exist in the log being replayed, and there's no messages on those
+// channels as well.
+TEST_F(RealtimeMultiNodeLoggerTest, DoesNotExistInReplayChannelsTest) {
+ FLAGS_override_hostname = "raspberrypi";
+ {
+ std::unique_ptr<EventLoop> logger_event_loop =
+ event_loop_factory_.MakeEventLoop(
+ "logger", configuration::GetNode(&config_.message(), "pi1"));
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(95));
+
+ Logger logger(logger_event_loop.get());
+ logger.set_separate_config(false);
+ logger.set_polling_period(std::chrono::milliseconds(100));
+ std::unique_ptr<MultiNodeLogNamer> namer =
+ std::make_unique<MultiNodeLogNamer>(
+ base_name_, &config_.message(), logger_event_loop.get(),
+ configuration::GetNode(&config_.message(), "pi1"));
+
+ logger.StartLogging(std::move(namer));
+ event_loop_factory_.RunFor(std::chrono::milliseconds(2000));
+ }
+
+ ReplayChannels replay_channels{{"/test", "aos.examples.Pong"},
+ {"/test", "fake"},
+ {"fake", "aos.examples.Ping"}};
+ LogReader reader(logger::SortParts(logger::FindLogs(base_name_)),
+ &config_.message(), &replay_channels);
+ ShmEventLoop shm_event_loop(reader.configuration());
+ reader.Register(&shm_event_loop);
+ reader.OnEnd(shm_event_loop.node(),
+ [&shm_event_loop]() { shm_event_loop.Exit(); });
+
+ Fetcher<examples::Ping> ping_fetcher =
+ shm_event_loop.MakeFetcher<examples::Ping>("/test");
+
+ auto *const end_timer = shm_event_loop.AddTimer([&shm_event_loop]() {
+ LOG(INFO) << "All done, quitting now";
+ shm_event_loop.Exit();
+ });
+
+ // TODO(#21) reader.OnEnd() is not working as expected when
+ // using replay_channels
+ // keep looking for 3 seconds if some message comes, just in case
+ size_t run_seconds = 3;
+ shm_event_loop.OnRun([&shm_event_loop, end_timer, run_seconds]() {
+ LOG(INFO) << "Quitting in: " << run_seconds;
+ end_timer->Setup(shm_event_loop.monotonic_now() +
+ std::chrono::seconds(run_seconds));
+ });
+
+ shm_event_loop.Run();
+ reader.Deregister();
+ ASSERT_FALSE(ping_fetcher.Fetch());
+}
+
+using RealtimeMultiNodeLoggerDeathTest = RealtimeMultiNodeLoggerTest;
+
+// Tests that remapping a channel not included in the replay channels passed to
+// LogReader throws an error since this would indicate the user is trying to use
+// the channel being remapped.
+TEST_F(RealtimeMultiNodeLoggerDeathTest,
+ RemapLoggedChannelNotIncludedInReplayChannels) {
+ FLAGS_override_hostname = "raspberrypi";
+ {
+ std::unique_ptr<EventLoop> logger_event_loop =
+ event_loop_factory_.MakeEventLoop(
+ "logger", configuration::GetNode(&config_.message(), "pi1"));
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(95));
+
+ Logger logger(logger_event_loop.get());
+ logger.set_separate_config(false);
+ logger.set_polling_period(std::chrono::milliseconds(100));
+
+ std::unique_ptr<MultiNodeLogNamer> namer =
+ std::make_unique<MultiNodeLogNamer>(
+ base_name_, &config_.message(), logger_event_loop.get(),
+ configuration::GetNode(&config_.message(), "pi1"));
+
+ logger.StartLogging(std::move(namer));
+ event_loop_factory_.RunFor(std::chrono::milliseconds(2000));
+ }
+
+ ReplayChannels replay_channels{{"/test", "aos.examples.Ping"}};
+ LogReader reader(logger::SortParts(logger::FindLogs(base_name_)),
+ &config_.message(), &replay_channels);
+ EXPECT_DEATH(
+ reader.RemapLoggedChannel<aos::examples::Ping>("/fake", "/original"),
+ "which is not included in the replay channels passed to LogReader");
+}
+
} // namespace aos::logger::testing
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index 1352249..89d0a9c 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -599,6 +599,7 @@
if (repeat_offset_ == std::chrono::seconds(0)) {
timerfd_.Disable();
+ disabled_ = true;
} else {
// Compute how many cycles have elapsed and schedule the next iteration
// for the next iteration in the future.
@@ -612,6 +613,7 @@
event_.set_event_time(base_);
shm_event_loop_->AddEvent(&event_);
timerfd_.SetTime(base_, std::chrono::seconds(0));
+ disabled_ = false;
}
}
@@ -627,6 +629,7 @@
repeat_offset_ = repeat_offset;
event_.set_event_time(base_);
shm_event_loop_->AddEvent(&event_);
+ disabled_ = false;
}
void Disable() override {
@@ -636,6 +639,8 @@
disabled_ = true;
}
+ bool IsDisabled() override { return disabled_; }
+
private:
ShmEventLoop *shm_event_loop_;
EventHandler<ShmTimerHandler> event_;
@@ -647,7 +652,7 @@
// Used to track if Disable() was called during the callback, so we know not
// to reschedule.
- bool disabled_ = false;
+ bool disabled_ = true;
};
// Adapter class to the timerfd and PhasedLoop.
@@ -674,9 +679,7 @@
timerfd_.Read();
event_.Invalidate();
- Call(monotonic_clock::now, [this](monotonic_clock::time_point sleep_time) {
- Schedule(sleep_time);
- });
+ Call(monotonic_clock::now);
}
~ShmPhasedLoopHandler() override {
@@ -819,7 +822,8 @@
} else {
next_time = PeekEvent()->event_time();
}
- const auto now = monotonic_clock::now();
+ monotonic_clock::time_point now;
+ bool new_data = false;
if (next_time > checked_until) {
// Read all of the signals, because there's no point in waking up again
@@ -835,13 +839,19 @@
}
CHECK_EQ(result.ssi_signo, ipc_lib::kWakeupSignal);
}
+ // This is the last time we can guarantee that if a message is published
+ // before, we will notice it.
+ now = monotonic_clock::now();
// Check all the watchers for new events.
for (std::unique_ptr<WatcherState> &base_watcher : watchers_) {
ShmWatcherState *const watcher =
reinterpret_cast<ShmWatcherState *>(base_watcher.get());
- watcher->CheckForNewData();
+ // Track if we got a message.
+ if (watcher->CheckForNewData()) {
+ new_data = true;
+ }
}
if (EventCount() == 0) {
// Still no events, all done now.
@@ -851,9 +861,19 @@
checked_until = now;
// Check for any new events we found.
next_time = PeekEvent()->event_time();
+ } else {
+ now = monotonic_clock::now();
}
if (next_time > now) {
+ // Ok, we got a message with a timestamp *after* we wrote down time. We
+ // need to process it (otherwise we will go to sleep without processing
+ // it), but we also need to make sure no other messages have come in
+ // before it that we would process out of order. Just go around again to
+ // redo the checks.
+ if (new_data) {
+ continue;
+ }
break;
}
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 944a22b..c679b21 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -521,6 +521,8 @@
void Disable() override;
+ bool IsDisabled() override;
+
private:
SimulatedEventLoop *simulated_event_loop_;
EventHandler<SimulatedTimerHandler> event_;
@@ -529,6 +531,7 @@
monotonic_clock::time_point base_;
monotonic_clock::duration repeat_offset_;
+ bool disabled_ = true;
};
class SimulatedPhasedLoopHandler : public PhasedLoopHandler,
@@ -1201,6 +1204,7 @@
token_ = scheduler_->Schedule(std::max(base, monotonic_now), this);
event_.set_event_time(base_);
simulated_event_loop_->AddEvent(&event_);
+ disabled_ = false;
}
void SimulatedTimerHandler::Handle() noexcept {
@@ -1232,8 +1236,10 @@
token_ = scheduler_->Schedule(base_, this);
event_.set_event_time(base_);
simulated_event_loop_->AddEvent(&event_);
+ disabled_ = false;
+ } else {
+ disabled_ = true;
}
-
{
ScopedMarkRealtimeRestorer rt(
simulated_event_loop_->runtime_realtime_priority() > 0);
@@ -1251,8 +1257,11 @@
}
token_ = scheduler_->InvalidToken();
}
+ disabled_ = true;
}
+bool SimulatedTimerHandler::IsDisabled() { return disabled_; }
+
SimulatedPhasedLoopHandler::SimulatedPhasedLoopHandler(
EventScheduler *scheduler, SimulatedEventLoop *simulated_event_loop,
::std::function<void(int)> fn, const monotonic_clock::duration interval,
@@ -1284,10 +1293,7 @@
{
ScopedMarkRealtimeRestorer rt(
simulated_event_loop_->runtime_realtime_priority() > 0);
- Call([monotonic_now]() { return monotonic_now; },
- [this](monotonic_clock::time_point sleep_time) {
- Schedule(sleep_time);
- });
+ Call([monotonic_now]() { return monotonic_now; });
simulated_event_loop_->ClearContext();
}
}
@@ -1303,6 +1309,7 @@
// The allocations in here are due to infrastructure and don't count in the no
// mallocs in RT code.
ScopedNotRealtime nrt;
+ simulated_event_loop_->RemoveEvent(&event_);
if (token_ != scheduler_->InvalidToken()) {
scheduler_->Deschedule(token_);
token_ = scheduler_->InvalidToken();
diff --git a/aos/events/simulated_event_loop.h b/aos/events/simulated_event_loop.h
index 22eedc1..c4704ff 100644
--- a/aos/events/simulated_event_loop.h
+++ b/aos/events/simulated_event_loop.h
@@ -234,13 +234,13 @@
// args for the Main class. Returns a pointer to the class that was started
// if it was started, or nullptr.
template <class Main, class... Args>
- Main *MaybeStart(std::string_view name, Args &&...args);
+ Main *MaybeStart(std::string_view name, Args &&... args);
// Starts an application regardless of if the config says to or not. name is
// the name of the application, and args are the constructor args for the
// application. Returns a pointer to the class that was started.
template <class Main, class... Args>
- Main *AlwaysStart(std::string_view name, Args &&...args);
+ Main *AlwaysStart(std::string_view name, Args &&... args);
// Returns the simulated network delay for messages forwarded between nodes.
std::chrono::nanoseconds network_delay() const {
@@ -252,6 +252,8 @@
size_t boot_count() const { return scheduler_.boot_count(); }
+ bool is_running() const { return scheduler_.is_running(); }
+
// TODO(austin): Private for the following?
// Converts a time to the distributed clock for scheduling and cross-node time
@@ -348,7 +350,7 @@
// application.
template <class... Args>
TypedApplication(NodeEventLoopFactory *node_factory, std::string_view name,
- Args &&...args)
+ Args &&... args)
: Application(node_factory, name),
main(event_loop.get(), std::forward<Args>(args)...) {
VLOG(1) << node_factory->scheduler_.distributed_now() << " "
@@ -367,7 +369,7 @@
};
template <class Main, class... Args>
-Main *NodeEventLoopFactory::MaybeStart(std::string_view name, Args &&...args) {
+Main *NodeEventLoopFactory::MaybeStart(std::string_view name, Args &&... args) {
const aos::Application *application =
configuration::GetApplication(configuration(), node(), name);
@@ -378,7 +380,8 @@
}
template <class Main, class... Args>
-Main *NodeEventLoopFactory::AlwaysStart(std::string_view name, Args &&...args) {
+Main *NodeEventLoopFactory::AlwaysStart(std::string_view name,
+ Args &&... args) {
std::unique_ptr<TypedApplication<Main>> app =
std::make_unique<TypedApplication<Main>>(this, name,
std::forward<Args>(args)...);
diff --git a/aos/flatbuffers.bzl b/aos/flatbuffers.bzl
index 3db4ca0..c26906a 100644
--- a/aos/flatbuffers.bzl
+++ b/aos/flatbuffers.bzl
@@ -1,16 +1,18 @@
-def cc_static_flatbuffer(name, target, function, visibility = None):
+def cc_static_flatbuffer(name, target, function, bfbs_name = None, visibility = None):
"""Creates a cc_library which encodes a file as a Span.
args:
target, The file to encode.
function, The inline function, with full namespaces, to create.
+ bfbs_name, For flatbuffer targets that have multiple fbs files, this
+ specifies the basename of the bfbs file to generate a schema for.
"""
native.genrule(
name = name + "_gen",
tools = ["@org_frc971//aos:flatbuffers_static"],
srcs = [target],
outs = [name + ".h"],
- cmd = "$(location @org_frc971//aos:flatbuffers_static) $(SRCS) $(OUTS) '" + function + "'",
+ cmd = "$(location @org_frc971//aos:flatbuffers_static) '$(SRCS)' $(OUTS) '" + function + "' " + (bfbs_name if bfbs_name else "-"),
)
native.cc_library(
diff --git a/aos/flatbuffers_static.py b/aos/flatbuffers_static.py
index 199c5b9..3bd28fe 100644
--- a/aos/flatbuffers_static.py
+++ b/aos/flatbuffers_static.py
@@ -8,12 +8,26 @@
def main(argv):
- if len(argv) != 4:
+ if len(argv) != 5:
+ print(
+ f"Incorrect number of arguments {len(argv)} to flatbuffers_static."
+ )
+ print(argv)
return 1
input_path = sys.argv[1]
output_path = sys.argv[2]
function = sys.argv[3].split("::")
+ bfbs_name = sys.argv[4]
+ if bfbs_name != '-':
+ inputs = input_path.split(' ')
+ valid_paths = [path for path in inputs if path.endswith(bfbs_name)]
+ if len(valid_paths) != 1:
+ print(
+ f"Expected exactly one match for {bfbs_name}; got {valid_paths}."
+ )
+ return 1
+ input_path = valid_paths[0]
include_guard = output_path.replace('/', '_').replace('-', '_').replace(
'.', '_').upper() + '_'
diff --git a/aos/json_to_flatbuffer.cc b/aos/json_to_flatbuffer.cc
index 825bb3c..fa1cf62 100644
--- a/aos/json_to_flatbuffer.cc
+++ b/aos/json_to_flatbuffer.cc
@@ -215,6 +215,14 @@
return false;
}
+ if (in_vector() != stack_.back().type.FieldIsRepeating(field_index)) {
+ fprintf(stderr,
+ "Field '%s' is%s supposed to be a vector, but is a %s.\n",
+ stack_.back().field_name.c_str(), in_vector() ? " not" : "",
+ in_vector() ? "vector" : "bare object");
+ return false;
+ }
+
stack_.push_back({stack_.back().type.FieldType(field_index),
false,
-1,
diff --git a/aos/json_to_flatbuffer_test.cc b/aos/json_to_flatbuffer_test.cc
index 4918281..6772826 100644
--- a/aos/json_to_flatbuffer_test.cc
+++ b/aos/json_to_flatbuffer_test.cc
@@ -215,6 +215,13 @@
EXPECT_TRUE(JsonAndBack("{ \"apps\": [ { }, { } ] }"));
}
+// Test mixing up whether a field is an object or a vector.
+TEST_F(JsonToFlatbufferTest, IncorrectVectorOfTables) {
+ EXPECT_FALSE(
+ JsonAndBack("{ \"single_application\": [ {\"name\": \"woot\"} ] }"));
+ EXPECT_FALSE(JsonAndBack("{ \"apps\": { \"name\": \"woot\" } }"));
+}
+
// Test that we can parse an empty message.
TEST_F(JsonToFlatbufferTest, EmptyMessage) {
// Empty message works.
diff --git a/aos/network/BUILD b/aos/network/BUILD
index 96825e4..4794838 100644
--- a/aos/network/BUILD
+++ b/aos/network/BUILD
@@ -375,6 +375,21 @@
deps = ["//aos/events:aos_config"],
)
+aos_config(
+ name = "timestamp_channel_test_config",
+ src = "timestamp_channel_test.json",
+ flatbuffers = [
+ ":remote_message_fbs",
+ "//aos/events:ping_fbs",
+ "//aos/events:pong_fbs",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = ["//aos/events:aos_config"],
+)
+
cc_test(
name = "message_bridge_test",
srcs = [
@@ -616,3 +631,19 @@
"//aos/testing:googletest",
],
)
+
+cc_test(
+ name = "timestamp_channel_test",
+ srcs = ["timestamp_channel_test.cc"],
+ data = [":timestamp_channel_test_config"],
+ deps = [
+ ":timestamp_channel",
+ "//aos:configuration",
+ "//aos/events:ping_fbs",
+ "//aos/events:shm_event_loop",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ "//aos/testing:path",
+ "//aos/testing:tmpdir",
+ ],
+)
diff --git a/aos/network/message_bridge_test_combined_timestamps_common.json b/aos/network/message_bridge_test_combined_timestamps_common.json
index be79014..5d82965 100644
--- a/aos/network/message_bridge_test_combined_timestamps_common.json
+++ b/aos/network/message_bridge_test_combined_timestamps_common.json
@@ -75,8 +75,7 @@
{
"name": "/pi1/aos/remote_timestamps/pi2",
"type": "aos.message_bridge.RemoteMessage",
- "source_node": "pi1",
- "frequency": 15
+ "source_node": "pi1"
},
{
"name": "/pi2/aos/remote_timestamps/pi1",
diff --git a/aos/network/message_bridge_test_common.json b/aos/network/message_bridge_test_common.json
index 4623edb..a30734d 100644
--- a/aos/network/message_bridge_test_common.json
+++ b/aos/network/message_bridge_test_common.json
@@ -87,20 +87,17 @@
{
"name": "/pi1/aos/remote_timestamps/pi2/test/aos-examples-Ping",
"type": "aos.message_bridge.RemoteMessage",
- "source_node": "pi1",
- "frequency": 15
+ "source_node": "pi1"
},
{
"name": "/pi2/aos/remote_timestamps/pi1/test/aos-examples-Pong",
"type": "aos.message_bridge.RemoteMessage",
- "source_node": "pi2",
- "frequency": 15
+ "source_node": "pi2"
},
{
"name": "/pi1/aos/remote_timestamps/pi2/unreliable/aos-examples-Ping",
"type": "aos.message_bridge.RemoteMessage",
- "source_node": "pi1",
- "frequency": 15
+ "source_node": "pi1"
},
{
"name": "/pi1/aos",
diff --git a/aos/network/timestamp_channel.cc b/aos/network/timestamp_channel.cc
index fdaa031..f8f525f 100644
--- a/aos/network/timestamp_channel.cc
+++ b/aos/network/timestamp_channel.cc
@@ -109,6 +109,13 @@
const Channel *timestamp_channel = finder.ForChannel(channel, connection);
+ // Sanity-check that the timestamp channel can actually support full-rate
+ // messages coming through on the source channel.
+ CHECK_GE(timestamp_channel->frequency(), channel->frequency())
+ << ": Timestamp channel "
+ << configuration::StrippedChannelToString(timestamp_channel)
+ << "'s rate is lower than the source channel.";
+
{
auto it = timestamp_loggers_.find(timestamp_channel);
if (it != timestamp_loggers_.end()) {
diff --git a/aos/network/timestamp_channel_test.cc b/aos/network/timestamp_channel_test.cc
new file mode 100644
index 0000000..e3850ca
--- /dev/null
+++ b/aos/network/timestamp_channel_test.cc
@@ -0,0 +1,71 @@
+#include "aos/network/timestamp_channel.h"
+
+#include "aos/configuration.h"
+#include "aos/events/ping_generated.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+#include "aos/testing/tmpdir.h"
+#include "gtest/gtest.h"
+
+DECLARE_string(override_hostname);
+
+namespace aos::message_bridge::testing {
+class TimestampChannelTest : public ::testing::Test {
+ protected:
+ TimestampChannelTest()
+ : config_(aos::configuration::ReadConfig(aos::testing::ArtifactPath(
+ "aos/network/timestamp_channel_test_config.json"))) {
+ FLAGS_shm_base = aos::testing::TestTmpDir();
+ FLAGS_override_hostname = "pi1";
+ }
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+};
+
+// Tests that creating a SimulatedEventLoopFactory with invalid remote timestamp
+// channel frequencies fails.
+TEST_F(TimestampChannelTest, SimulatedNetworkBridgeFrequencyMismatch) {
+ SimulatedEventLoopFactory factory(&config_.message());
+ EXPECT_DEATH(factory.RunFor(std::chrono::seconds(1)),
+ "rate is lower than the source channel");
+}
+
+class TimestampChannelParamTest
+ : public TimestampChannelTest,
+ public ::testing::WithParamInterface<
+ std::tuple<std::string, std::optional<std::string>>> {};
+
+// Tests whether we can or can't retrieve a timestamp channel depending on
+// whether it has a valid max frequency configured.
+TEST_P(TimestampChannelParamTest, ChannelFrequency) {
+ aos::ShmEventLoop event_loop(&config_.message());
+ ChannelTimestampSender timestamp_sender(&event_loop);
+ const aos::Channel *channel =
+ event_loop.GetChannel<aos::examples::Ping>(std::get<0>(GetParam()));
+ const std::optional<std::string> error_message = std::get<1>(GetParam());
+ if (error_message.has_value()) {
+ EXPECT_DEATH(timestamp_sender.SenderForChannel(
+ channel, channel->destination_nodes()->Get(0)),
+ error_message.value());
+ } else {
+ aos::Sender<RemoteMessage> *sender = timestamp_sender.SenderForChannel(
+ channel, channel->destination_nodes()->Get(0));
+ ASSERT_TRUE(sender != nullptr);
+ EXPECT_EQ(absl::StrCat("/pi1/aos/remote_timestamps/pi2",
+ std::get<0>(GetParam()), "/aos-examples-Ping"),
+ sender->channel()->name()->string_view());
+ }
+}
+
+std::tuple<std::string, std::optional<std::string>> MakeParams(
+ std::string channel, std::optional<std::string> error) {
+ return std::make_tuple(channel, error);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+ ChannelFrequencyTest, TimestampChannelParamTest,
+ ::testing::Values(MakeParams("/nominal", std::nullopt),
+ MakeParams("/timestamps_too_fast", std::nullopt),
+ MakeParams("/timestamps_too_slow",
+ "rate is lower than the source channel")));
+} // namespace aos::message_bridge::testing
diff --git a/aos/network/timestamp_channel_test.json b/aos/network/timestamp_channel_test.json
new file mode 100644
index 0000000..f60cae8
--- /dev/null
+++ b/aos/network/timestamp_channel_test.json
@@ -0,0 +1,195 @@
+{
+ "channels": [
+ {
+ "name": "/pi1/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi1",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "pi2",
+ "frequency": 200,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/pi1/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "pi1",
+ "frequency": 15,
+ "max_size": 200,
+ "destination_nodes": [
+ {
+ "name": "pi2",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["pi1"],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "pi2",
+ "frequency": 15,
+ "max_size": 200,
+ "destination_nodes": [
+ {
+ "name": "pi1",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["pi2"],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/pi1/aos/remote_timestamps/pi2/pi1/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "pi1",
+ "frequency": 15
+ },
+ {
+ "name": "/pi2/aos/remote_timestamps/pi1/pi2/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "pi2",
+ "frequency": 15
+ },
+ {
+ "name": "/pi1/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "pi1",
+ "frequency": 2
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "pi2",
+ "frequency": 2
+ },
+ {
+ "name": "/pi1/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "pi1",
+ "frequency": 15
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "pi2",
+ "frequency": 15
+ },
+ {
+ "name": "/pi1/aos",
+ "type": "aos.timing.Report",
+ "source_node": "pi1",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/pi2/aos",
+ "type": "aos.timing.Report",
+ "source_node": "pi2",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/pi1/aos/remote_timestamps/pi2/nominal/aos-examples-Ping",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "pi1",
+ "frequency": 15
+ },
+ {
+ "name": "/pi1/aos/remote_timestamps/pi2/timestamps_too_slow/aos-examples-Ping",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "pi1",
+ /* This frequency is configured lower than it should be. This will cause errors. */
+ "frequency": 5
+ },
+ {
+ "name": "/pi1/aos/remote_timestamps/pi2/timestamps_too_fast/aos-examples-Ping",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "pi1",
+ /* This frequency is configured higher than it should be. This should not cause errors. */
+ "frequency": 10000
+ },
+ {
+ "name": "/nominal",
+ "type": "aos.examples.Ping",
+ "source_node": "pi1",
+ "frequency": 15,
+ "destination_nodes": [
+ {
+ "name": "pi2",
+ "timestamp_logger": "REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["pi1"]
+ }
+ ]
+ },
+ {
+ "name": "/timestamps_too_slow",
+ "type": "aos.examples.Ping",
+ "source_node": "pi1",
+ "frequency": 15,
+ "destination_nodes": [
+ {
+ "name": "pi2",
+ "timestamp_logger": "REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["pi1"]
+ }
+ ]
+ },
+ {
+ "name": "/timestamps_too_fast",
+ "type": "aos.examples.Ping",
+ "source_node": "pi1",
+ "frequency": 15,
+ "destination_nodes": [
+ {
+ "name": "pi2",
+ "timestamp_logger": "REMOTE_LOGGER",
+ "timestamp_logger_nodes": ["pi1"]
+ }
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "pi1"
+ },
+ "rename": {
+ "name": "/pi1/aos"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "pi2"
+ },
+ "rename": {
+ "name": "/pi2/aos"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "pi1",
+ "hostname": "pi1",
+ "port": 9971
+ },
+ {
+ "name": "pi2",
+ "hostname": "pi2",
+ "port": 9972
+ }
+ ]
+}
diff --git a/aos/network/www/plotter.ts b/aos/network/www/plotter.ts
index 89b360c..17bffa3 100644
--- a/aos/network/www/plotter.ts
+++ b/aos/network/www/plotter.ts
@@ -342,8 +342,11 @@
// Make sure both have text in the right spot. Don't be too picky since
// nothing should really be changing here, and it's handy to let the
// user edit the HTML for testing.
- if (this.legend.children[child].lastChild.textContent.length == 0 &&
- line.label().length != 0) {
+ let textdiv = this.legend.children[child].lastChild;
+ let canvas = this.legend.children[child].firstChild;
+ if ((textdiv.textContent.length == 0 && line.label().length != 0) ||
+ (textdiv as HTMLDivElement).offsetHeight !=
+ (canvas as HTMLCanvasElement).height) {
needsUpdate = true;
break;
}
diff --git a/aos/realtime.h b/aos/realtime.h
index 55ef5f9..caf0daa 100644
--- a/aos/realtime.h
+++ b/aos/realtime.h
@@ -38,6 +38,15 @@
return result;
}
+// Returns the affinity representing all the CPUs.
+inline cpu_set_t DefaultAffinity() {
+ cpu_set_t result;
+ for (int i = 0; i < CPU_SETSIZE; ++i) {
+ CPU_SET(i, &result);
+ }
+ return result;
+}
+
// Returns the current thread's CPU affinity.
cpu_set_t GetCurrentThreadAffinity();
diff --git a/aos/util/BUILD b/aos/util/BUILD
index bd1dbab..929b376 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -1,4 +1,5 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
load("config_validator_macro.bzl", "config_validator_rule")
package(default_visibility = ["//visibility:public"])
@@ -46,7 +47,10 @@
name = "log_to_mcap",
srcs = ["log_to_mcap.cc"],
deps = [
+ ":clock_publisher",
+ ":clock_timepoints_schema",
":mcap_logger",
+ "//aos:configuration",
"//aos:init",
"//aos/events/logging:log_reader",
"//frc971/control_loops:control_loops_fbs",
@@ -75,6 +79,29 @@
],
)
+flatbuffer_cc_library(
+ name = "clock_timepoints_fbs",
+ srcs = ["clock_timepoints.fbs"],
+ gen_reflections = True,
+ deps = ["//aos:configuration_fbs"],
+)
+
+cc_static_flatbuffer(
+ name = "clock_timepoints_schema",
+ function = "aos::ClockTimepointsSchema",
+ target = ":clock_timepoints_fbs_reflection_out",
+)
+
+cc_library(
+ name = "clock_publisher",
+ srcs = ["clock_publisher.cc"],
+ hdrs = ["clock_publisher.h"],
+ deps = [
+ ":clock_timepoints_fbs",
+ "//aos/events:simulated_event_loop",
+ ],
+)
+
cc_library(
name = "mcap_logger",
srcs = ["mcap_logger.cc"],
@@ -336,6 +363,7 @@
":file",
"//aos:realtime",
"//aos/testing:googletest",
+ "//aos/testing:tmpdir",
],
)
@@ -438,6 +466,28 @@
)
cc_library(
+ name = "threaded_consumer",
+ hdrs = [
+ "threaded_consumer.h",
+ ],
+ deps = [
+ "//aos:condition",
+ "//aos:realtime",
+ "//aos/containers:ring_buffer",
+ "//aos/mutex",
+ ],
+)
+
+cc_test(
+ name = "threaded_consumer_test",
+ srcs = ["threaded_consumer_test.cc"],
+ deps = [
+ ":threaded_consumer",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
name = "foxglove_websocket_lib",
srcs = ["foxglove_websocket_lib.cc"],
hdrs = ["foxglove_websocket_lib.h"],
diff --git a/aos/util/clock_publisher.cc b/aos/util/clock_publisher.cc
new file mode 100644
index 0000000..93102f6
--- /dev/null
+++ b/aos/util/clock_publisher.cc
@@ -0,0 +1,51 @@
+#include "aos/util/clock_publisher.h"
+
+namespace aos {
+ClockPublisher::ClockPublisher(aos::SimulatedEventLoopFactory *factory,
+ aos::EventLoop *event_loop)
+ : factory_(factory),
+ timepoints_sender_(event_loop->MakeSender<ClockTimepoints>("/clocks")) {
+ aos::TimerHandler *timer_handler =
+ event_loop->AddTimer([this]() { SendTimepoints(); });
+ event_loop->OnRun([timer_handler, event_loop]() {
+ timer_handler->Setup(event_loop->context().monotonic_event_time,
+ std::chrono::seconds(1));
+ });
+}
+
+void ClockPublisher::SendTimepoints() {
+ std::vector<flatbuffers::Offset<NodeTimepoint>> timepoints;
+ auto builder = timepoints_sender_.MakeBuilder();
+ for (const aos::Node *node : factory_->nodes()) {
+ const NodeEventLoopFactory *node_factory =
+ factory_->GetNodeEventLoopFactory(node);
+ flatbuffers::Offset<flatbuffers::String> node_name =
+ (node != nullptr)
+ ? builder.fbb()->CreateString(node->name()->string_view())
+ : flatbuffers::Offset<flatbuffers::String>(0);
+ NodeTimepoint::Builder timepoint_builder =
+ builder.MakeBuilder<NodeTimepoint>();
+ if (node != nullptr) {
+ timepoint_builder.add_node(node_name);
+ }
+ if (node_factory->is_running()) {
+ timepoint_builder.add_boot_count(node_factory->boot_count());
+ timepoint_builder.add_monotonic_time(
+ node_factory->monotonic_now().time_since_epoch().count());
+ timepoint_builder.add_realtime_time(
+ node_factory->realtime_now().time_since_epoch().count());
+ }
+ timepoints.push_back(timepoint_builder.Finish());
+ }
+ const flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<NodeTimepoint>>>
+ timepoints_offset = builder.fbb()->CreateVector(timepoints);
+ ClockTimepoints::Builder timepoints_builder =
+ builder.MakeBuilder<ClockTimepoints>();
+ timepoints_builder.add_distributed_clock(
+ factory_->distributed_now().time_since_epoch().count());
+ timepoints_builder.add_clocks(timepoints_offset);
+ builder.CheckOk(builder.Send(timepoints_builder.Finish()));
+}
+
+} // namespace aos
diff --git a/aos/util/clock_publisher.h b/aos/util/clock_publisher.h
new file mode 100644
index 0000000..a6d3488
--- /dev/null
+++ b/aos/util/clock_publisher.h
@@ -0,0 +1,26 @@
+#ifndef AOS_UTIL_CLOCK_PUBLISHER_H_
+#define AOS_UTIL_CLOCK_PUBLISHER_H_
+#include "aos/events/simulated_event_loop.h"
+#include "aos/util/clock_timepoints_generated.h"
+
+namespace aos {
+// A simple class that periodically queries a SimulatedEventLoopFactory for the
+// current timestamps on all nodes and publishes a ClockTimepoints message on
+// the provided EventLoop.
+// This is used by the log_to_mcap converter to allow Foxglove users access to
+// offset estimates. In order to use this, a /clocks channel with a type of
+// aos.ClockTimepoints must be available.
+class ClockPublisher {
+ public:
+ ClockPublisher(aos::SimulatedEventLoopFactory *factory,
+ aos::EventLoop *event_loop);
+
+ private:
+ void SendTimepoints();
+
+ aos::SimulatedEventLoopFactory *const factory_;
+ aos::Sender<ClockTimepoints> timepoints_sender_;
+};
+} // namespace aos
+
+#endif // AOS_UTIL_CLOCK_PUBLISHER_H_
diff --git a/aos/util/clock_timepoints.fbs b/aos/util/clock_timepoints.fbs
new file mode 100644
index 0000000..892b1c4
--- /dev/null
+++ b/aos/util/clock_timepoints.fbs
@@ -0,0 +1,27 @@
+include "aos/configuration.fbs";
+
+namespace aos;
+
+// Current clock values on a given node. Clock values + boot count will not be populated if
+// the node is not currently running.
+table NodeTimepoint {
+ // The name of the node that this clock corresponds to.
+ node:string (id: 0);
+ // Current boot count for this node (to allow observing reboots).
+ boot_count:int (id: 1);
+ // Current monotonic time of this clock, in nanoseconds.
+ monotonic_time:int64 (id: 2);
+ // Current realtime (UNIX epoch) time of this clock, in nanoseconds.
+ realtime_time:int64 (id: 3);
+}
+
+table ClockTimepoints {
+ // Current "distributed clock" time, in nanoseconds. This will roughly correspond to the
+ // average of the monotonic clocks across all devices, and will itself be monotonic.
+ distributed_clock:int64 (id: 0);
+ // Current clock values for every node. There will be an entry for every node, and the nodes
+ // will be in the same order as they are in in the config used to generated this message.
+ clocks:[NodeTimepoint] (id: 1);
+}
+
+root_type ClockTimepoints;
diff --git a/aos/util/file.cc b/aos/util/file.cc
index cdf0061..6c35627 100644
--- a/aos/util/file.cc
+++ b/aos/util/file.cc
@@ -36,20 +36,9 @@
void WriteStringToFileOrDie(const std::string_view filename,
const std::string_view contents,
mode_t permissions) {
- ScopedFD fd(open(::std::string(filename).c_str(),
- O_CREAT | O_WRONLY | O_TRUNC, permissions));
- PCHECK(fd.get() != -1) << ": opening " << filename;
- size_t size_written = 0;
- while (size_written != contents.size()) {
- const ssize_t result = write(fd.get(), contents.data() + size_written,
- contents.size() - size_written);
- PCHECK(result >= 0) << ": reading from " << filename;
- if (result == 0) {
- break;
- }
-
- size_written += result;
- }
+ FileWriter writer(filename, permissions);
+ writer.WriteBytesOrDie(
+ {reinterpret_cast<const uint8_t *>(contents.data()), contents.size()});
}
bool MkdirPIfSpace(std::string_view path, mode_t mode) {
@@ -176,5 +165,47 @@
return span;
}
+FileWriter::FileWriter(std::string_view filename, mode_t permissions)
+ : file_(open(::std::string(filename).c_str(), O_WRONLY | O_CREAT | O_TRUNC,
+ permissions)) {
+ PCHECK(file_.get() != -1) << ": opening " << filename;
+}
+
+FileWriter::WriteResult FileWriter::WriteBytes(
+ absl::Span<const uint8_t> bytes) {
+ size_t size_written = 0;
+ while (size_written != bytes.size()) {
+ const ssize_t result = write(file_.get(), bytes.data() + size_written,
+ bytes.size() - size_written);
+ if (result < 0) {
+ return {size_written, static_cast<int>(result)};
+ }
+ // Not really supposed to happen unless writing zero bytes without an error.
+ // See, e.g.,
+ // https://stackoverflow.com/questions/2176443/is-a-return-value-of-0-from-write2-in-c-an-error
+ if (result == 0) {
+ return {size_written, static_cast<int>(result)};
+ }
+
+ size_written += result;
+ }
+ return {size_written, static_cast<int>(size_written)};
+}
+
+FileWriter::WriteResult FileWriter::WriteBytes(std::string_view bytes) {
+ return WriteBytes(absl::Span<const uint8_t>{
+ reinterpret_cast<const uint8_t *>(bytes.data()), bytes.size()});
+}
+
+void FileWriter::WriteBytesOrDie(std::string_view bytes) {
+ WriteBytesOrDie(absl::Span<const uint8_t>{
+ reinterpret_cast<const uint8_t *>(bytes.data()), bytes.size()});
+}
+
+void FileWriter::WriteBytesOrDie(absl::Span<const uint8_t> bytes) {
+ PCHECK(bytes.size() == WriteBytes(bytes).bytes_written)
+ << ": Failed to write " << bytes.size() << " bytes.";
+}
+
} // namespace util
} // namespace aos
diff --git a/aos/util/file.h b/aos/util/file.h
index d2fb9fa..56479ce 100644
--- a/aos/util/file.h
+++ b/aos/util/file.h
@@ -80,6 +80,37 @@
char buffer_[kBufferSize];
};
+// Simple interface to allow opening a file for writing and then writing it
+// without any malloc's.
+// TODO(james): It may make sense to add a ReadBytes() interface here that can
+// take a memory buffer to fill, to avoid the templating required by the
+// self-managed buffer of FileReader<>.
+class FileWriter {
+ public:
+ // The result of an individual call to WriteBytes().
+ // Because WriteBytes() may repeatedly call write() when partial writes occur,
+ // it is possible for a non-zero number of bytes to have been written while
+ // still having an error because a late call to write() failed.
+ struct WriteResult {
+ // Total number of bytes successfully written to the file.
+ size_t bytes_written;
+ // If the write was successful (return_code > 0), equal to bytes_written.
+ // Otherwise, equal to the return value of the final call to write.
+ int return_code;
+ };
+
+ FileWriter(std::string_view filename, mode_t permissions = S_IRWXU);
+
+ WriteResult WriteBytes(absl::Span<const uint8_t> bytes);
+ WriteResult WriteBytes(std::string_view bytes);
+ void WriteBytesOrDie(absl::Span<const uint8_t> bytes);
+ void WriteBytesOrDie(std::string_view bytes);
+ int fd() const { return file_.get(); }
+
+ private:
+ aos::ScopedFD file_;
+};
+
} // namespace util
} // namespace aos
diff --git a/aos/util/file_test.cc b/aos/util/file_test.cc
index 8e76154..9b0def0 100644
--- a/aos/util/file_test.cc
+++ b/aos/util/file_test.cc
@@ -3,8 +3,9 @@
#include <cstdlib>
#include <string>
-#include "gtest/gtest.h"
#include "aos/realtime.h"
+#include "aos/testing/tmpdir.h"
+#include "gtest/gtest.h"
DECLARE_bool(die_on_malloc);
@@ -14,7 +15,7 @@
// Basic test of reading a normal file.
TEST(FileTest, ReadNormalFile) {
- const ::std::string tmpdir(getenv("TEST_TMPDIR"));
+ const ::std::string tmpdir(aos::testing::TestTmpDir());
const ::std::string test_file = tmpdir + "/test_file";
ASSERT_EQ(0, system(("echo contents > " + test_file).c_str()));
EXPECT_EQ("contents\n", ReadFileToStringOrDie(test_file));
@@ -30,7 +31,7 @@
// Tests that the PathExists function works under normal conditions.
TEST(FileTest, PathExistsTest) {
- const std::string tmpdir(getenv("TEST_TMPDIR"));
+ const std::string tmpdir(aos::testing::TestTmpDir());
const std::string test_file = tmpdir + "/test_file";
// Make sure the test_file doesn't exist.
unlink(test_file.c_str());
@@ -43,17 +44,65 @@
// Basic test of reading a normal file.
TEST(FileTest, ReadNormalFileNoMalloc) {
- const ::std::string tmpdir(getenv("TEST_TMPDIR"));
+ const ::std::string tmpdir(aos::testing::TestTmpDir());
const ::std::string test_file = tmpdir + "/test_file";
- ASSERT_EQ(0, system(("echo 971 > " + test_file).c_str()));
+ // Make sure to include a string long enough to avoid small string
+ // optimization.
+ ASSERT_EQ(0, system(("echo 123456789 > " + test_file).c_str()));
FileReader reader(test_file);
+ gflags::FlagSaver flag_saver;
FLAGS_die_on_malloc = true;
RegisterMallocHook();
aos::ScopedRealtime realtime;
- EXPECT_EQ("971\n", reader.ReadContents());
- EXPECT_EQ(971, reader.ReadInt());
+ EXPECT_EQ("123456789\n", reader.ReadContents());
+ EXPECT_EQ(123456789, reader.ReadInt());
+}
+
+// Tests that we can write to a file without malloc'ing.
+TEST(FileTest, WriteNormalFileNoMalloc) {
+ const ::std::string tmpdir(aos::testing::TestTmpDir());
+ const ::std::string test_file = tmpdir + "/test_file";
+
+ FileWriter writer(test_file);
+
+ gflags::FlagSaver flag_saver;
+ FLAGS_die_on_malloc = true;
+ RegisterMallocHook();
+ FileWriter::WriteResult result;
+ {
+ aos::ScopedRealtime realtime;
+ result = writer.WriteBytes("123456789");
+ }
+ EXPECT_EQ(9, result.bytes_written);
+ EXPECT_EQ(9, result.return_code);
+ EXPECT_EQ("123456789", ReadFileToStringOrDie(test_file));
+}
+
+// Tests that if we fail to write a file that the error code propagates
+// correctly.
+TEST(FileTest, WriteFileError) {
+ const ::std::string tmpdir(aos::testing::TestTmpDir());
+ const ::std::string test_file = tmpdir + "/test_file";
+
+ // Open with only read permissions; this should cause things to fail.
+ FileWriter writer(test_file, S_IRUSR);
+
+ // Mess up the file management by closing the file descriptor.
+ PCHECK(0 == close(writer.fd()));
+
+ gflags::FlagSaver flag_saver;
+ FLAGS_die_on_malloc = true;
+ RegisterMallocHook();
+ FileWriter::WriteResult result;
+ {
+ aos::ScopedRealtime realtime;
+ result = writer.WriteBytes("123456789");
+ }
+ EXPECT_EQ(0, result.bytes_written);
+ EXPECT_EQ(-1, result.return_code);
+ EXPECT_EQ("", ReadFileToStringOrDie(test_file));
}
} // namespace testing
diff --git a/aos/util/log_to_mcap.cc b/aos/util/log_to_mcap.cc
index e669658..38a3be7 100644
--- a/aos/util/log_to_mcap.cc
+++ b/aos/util/log_to_mcap.cc
@@ -1,6 +1,9 @@
+#include "aos/configuration.h"
#include "aos/events/event_loop_generated.h"
#include "aos/events/logging/log_reader.h"
#include "aos/init.h"
+#include "aos/util/clock_publisher.h"
+#include "aos/util/clock_timepoints_schema.h"
#include "aos/util/mcap_logger.h"
DEFINE_string(node, "", "Node to replay from the perspective of.");
@@ -11,6 +14,9 @@
"If set, use full channel names; by default, will shorten names to be the "
"shortest possible version of the name (e.g., /aos instead of /pi/aos).");
DEFINE_bool(compress, true, "Whether to use LZ4 compression in MCAP file.");
+DEFINE_bool(include_clocks, true,
+ "Whether to add a /clocks channel that publishes all nodes' clock "
+ "offsets.");
// Converts an AOS log to an MCAP log that can be fed into Foxglove. To try this
// out, run:
@@ -40,9 +46,24 @@
replay_node = logger_node;
}
- aos::logger::LogReader reader(logfiles);
+ std::optional<aos::FlatbufferDetachedBuffer<aos::Configuration>> config;
- reader.Register();
+ if (FLAGS_include_clocks) {
+ aos::logger::LogReader config_reader(logfiles);
+
+ const aos::Configuration *raw_config = config_reader.logged_configuration();
+ config = aos::configuration::AddChannelToConfiguration(
+ raw_config, "/clocks",
+ aos::FlatbufferSpan<reflection::Schema>(aos::ClockTimepointsSchema()),
+ replay_node.empty()
+ ? nullptr
+ : aos::configuration::GetNode(raw_config, replay_node));
+ }
+
+ aos::logger::LogReader reader(
+ logfiles, config.has_value() ? &config.value().message() : nullptr);
+ aos::SimulatedEventLoopFactory factory(reader.configuration());
+ reader.RegisterWithoutStarting(&factory);
const aos::Node *node =
(replay_node.empty() ||
@@ -50,17 +71,36 @@
? nullptr
: aos::configuration::GetNode(reader.configuration(), replay_node);
- std::unique_ptr<aos::EventLoop> mcap_event_loop =
- reader.event_loop_factory()->MakeEventLoop("mcap", node);
+ std::unique_ptr<aos::EventLoop> clock_event_loop;
+ std::unique_ptr<aos::ClockPublisher> clock_publisher;
+ if (FLAGS_include_clocks) {
+ reader.OnStart(node, [&clock_event_loop, &reader, &clock_publisher,
+ &factory, node]() {
+ clock_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("clock", node);
+ clock_publisher = std::make_unique<aos::ClockPublisher>(
+ &factory, clock_event_loop.get());
+ });
+ }
+
+ std::unique_ptr<aos::EventLoop> mcap_event_loop;
CHECK(!FLAGS_output_path.empty());
- aos::McapLogger relogger(
- mcap_event_loop.get(), FLAGS_output_path,
- FLAGS_mode == "flatbuffer" ? aos::McapLogger::Serialization::kFlatbuffer
- : aos::McapLogger::Serialization::kJson,
- FLAGS_canonical_channel_names
- ? aos::McapLogger::CanonicalChannelNames::kCanonical
- : aos::McapLogger::CanonicalChannelNames::kShortened,
- FLAGS_compress ? aos::McapLogger::Compression::kLz4
- : aos::McapLogger::Compression::kNone);
+ std::unique_ptr<aos::McapLogger> relogger;
+ factory.GetNodeEventLoopFactory(node)
+ ->OnStartup([&relogger, &mcap_event_loop, &reader, node]() {
+ mcap_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("mcap", node);
+ relogger = std::make_unique<aos::McapLogger>(
+ mcap_event_loop.get(), FLAGS_output_path,
+ FLAGS_mode == "flatbuffer"
+ ? aos::McapLogger::Serialization::kFlatbuffer
+ : aos::McapLogger::Serialization::kJson,
+ FLAGS_canonical_channel_names
+ ? aos::McapLogger::CanonicalChannelNames::kCanonical
+ : aos::McapLogger::CanonicalChannelNames::kShortened,
+ FLAGS_compress ? aos::McapLogger::Compression::kLz4
+ : aos::McapLogger::Compression::kNone);
+ });
reader.event_loop_factory()->Run();
+ reader.Deregister();
}
diff --git a/aos/util/mcap_logger.cc b/aos/util/mcap_logger.cc
index 742f284..96a9b60 100644
--- a/aos/util/mcap_logger.cc
+++ b/aos/util/mcap_logger.cc
@@ -229,15 +229,6 @@
// Manually add in a special /configuration channel.
if (register_handlers == RegisterHandlers::kYes) {
configuration_id_ = ++id;
- event_loop_->OnRun([this]() {
- Context config_context;
- config_context.monotonic_event_time = event_loop_->monotonic_now();
- config_context.queue_index = 0;
- config_context.size = configuration_.span().size();
- config_context.data = configuration_.span().data();
- WriteMessage(configuration_id_, &configuration_channel_.message(),
- config_context, ¤t_chunks_[configuration_id_]);
- });
}
std::vector<SummaryOffset> offsets;
@@ -274,6 +265,18 @@
return offsets;
}
+void McapLogger::WriteConfigurationMessage() {
+ Context config_context;
+ config_context.monotonic_event_time = event_loop_->monotonic_now();
+ config_context.queue_index = 0;
+ config_context.size = configuration_.span().size();
+ config_context.data = configuration_.span().data();
+ // Avoid infinite recursion...
+ wrote_configuration_ = true;
+ WriteMessage(configuration_id_, &configuration_channel_.message(),
+ config_context, ¤t_chunks_[configuration_id_]);
+}
+
void McapLogger::WriteMagic() { output_ << "\x89MCAP0\r\n"; }
void McapLogger::WriteHeader() {
@@ -388,6 +391,9 @@
void McapLogger::WriteMessage(uint16_t channel_id, const Channel *channel,
const Context &context, ChunkStatus *chunk) {
+ if (!wrote_configuration_) {
+ WriteConfigurationMessage();
+ }
CHECK_NOTNULL(context.data);
message_counts_[channel_id]++;
diff --git a/aos/util/mcap_logger.h b/aos/util/mcap_logger.h
index 70a1328..c3fdda1 100644
--- a/aos/util/mcap_logger.h
+++ b/aos/util/mcap_logger.h
@@ -143,6 +143,11 @@
const Context &context, ChunkStatus *chunk);
void WriteChunk(ChunkStatus *chunk);
+ // Writes out the special configuration channel. This gets called right before
+ // the first actual message is written so that we can have a reasonable
+ // monotonic clock time.
+ void WriteConfigurationMessage();
+
// The helpers for writing records which appear in the Summary section will
// return SummaryOffset's so that they can be referenced in the SummaryOffset
// section.
@@ -199,6 +204,7 @@
uint16_t configuration_id_ = 0;
FlatbufferDetachedBuffer<Channel> configuration_channel_;
FlatbufferDetachedBuffer<Configuration> configuration_;
+ bool wrote_configuration_ = false;
// Memory buffer to use for compressing data.
std::vector<uint8_t> compression_buffer_;
diff --git a/aos/util/phased_loop.cc b/aos/util/phased_loop.cc
index 53f6f4d..1f61775 100644
--- a/aos/util/phased_loop.cc
+++ b/aos/util/phased_loop.cc
@@ -17,15 +17,26 @@
void PhasedLoop::set_interval_and_offset(
const monotonic_clock::duration interval,
- const monotonic_clock::duration offset) {
+ const monotonic_clock::duration offset,
+ std::optional<monotonic_clock::time_point> monotonic_now) {
// Update last_time_ to the new offset so that we have an even interval
- last_time_ += offset - offset_;
+ // In doing so, set things so that last_time_ will only ever decrease on calls
+ // to set_interval_and_offset.
+ last_time_ += offset - offset_ -
+ (offset > offset_ ? interval : monotonic_clock::duration(0));
interval_ = interval;
offset_ = offset;
CHECK(offset_ >= monotonic_clock::duration(0));
CHECK(interval_ > monotonic_clock::duration(0));
CHECK(offset_ < interval_);
+ // Reset effectively clears the skipped iteration count and ensures that the
+ // last time is in the interval (monotonic_now - interval, monotonic_now],
+ // which means that a call to Iterate(monotonic_now) will return 1 and set a
+ // wakeup time after monotonic_now.
+ if (monotonic_now.has_value()) {
+ Iterate(monotonic_now.value());
+ }
}
monotonic_clock::duration PhasedLoop::OffsetFromIntervalAndTime(
diff --git a/aos/util/phased_loop.h b/aos/util/phased_loop.h
index 307b400..64a3302 100644
--- a/aos/util/phased_loop.h
+++ b/aos/util/phased_loop.h
@@ -1,6 +1,8 @@
#ifndef AOS_UTIL_PHASED_LOOP_H_
#define AOS_UTIL_PHASED_LOOP_H_
+#include <optional>
+
#include "aos/time/time.h"
namespace aos {
@@ -21,8 +23,30 @@
const monotonic_clock::duration offset = monotonic_clock::duration(0));
// Updates the offset and interval.
- void set_interval_and_offset(const monotonic_clock::duration interval,
- const monotonic_clock::duration offset);
+ //
+ // After a call to set_interval_and_offset with monotonic_now = nullopt, the
+ // following will hold, for any allowed values of interval and offset:
+ // auto original_time = loop.sleep_time();
+ // loop.set_interval_and_offset(interval, offset);
+ // CHECK_LE(loop.sleep_time(), original_time);
+ // CHECK_EQ(0, loop.Iterate(original_time));
+ //
+ // Note that this will not be the behavior that all (or even necessarily most)
+ // users want, since it doesn't necessarily preserve a "keep the iteration
+ // time as consistent as possible" concept. However, it *is* better defined
+ // than the alternative, where if you decrease the offset by, e.g., 1ms on a
+ // 100ms interval, then the behavior will vary depending on whather you are
+ // going from 0ms->999ms offset or from 1ms->0ms offset.
+ //
+ // If monotonic_now is set, then the following will hold:
+ // auto original_time = loop.sleep_time();
+ // loop.set_interval_and_offset(interval, offset, monotonic_now);
+ // CHECK_LE(loop.sleep_time(), monotonic_now);
+ // CHECK_EQ(0, loop.Iterate(monotonic_now));
+ void set_interval_and_offset(
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset,
+ std::optional<monotonic_clock::time_point> monotonic_now = std::nullopt);
// Computes the offset given an interval and a time that we should trigger.
static monotonic_clock::duration OffsetFromIntervalAndTime(
diff --git a/aos/util/phased_loop_test.cc b/aos/util/phased_loop_test.cc
index 5e85ea0..de4483c 100644
--- a/aos/util/phased_loop_test.cc
+++ b/aos/util/phased_loop_test.cc
@@ -1,6 +1,7 @@
#include "aos/util/phased_loop.h"
#include "aos/time/time.h"
+#include "glog/logging.h"
#include "gtest/gtest.h"
namespace aos {
@@ -230,7 +231,23 @@
ASSERT_EQ(5, loop.Iterate(last_time));
for (int i = 1; i < kCount; i++) {
const auto offset = kOffset - milliseconds(i);
- loop.set_interval_and_offset(kInterval, offset);
+ // First, set the interval/offset without specifying a "now". If we then
+ // attempt to Iterate() to the same time as the last iteration, this should
+ // always result in zero cycles elapsed.
+ {
+ const monotonic_clock::time_point original_time = loop.sleep_time();
+ loop.set_interval_and_offset(kInterval, offset);
+ EXPECT_EQ(original_time - milliseconds(1), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(last_time));
+ }
+
+ // Now, explicitly update/clear things to last_time. This should have the
+ // same behavior as not specifying a monotonic_now.
+ {
+ loop.set_interval_and_offset(kInterval, offset, last_time);
+ EXPECT_EQ(0, loop.Iterate(last_time));
+ }
+
const auto next_time = last_time - milliseconds(1) + kAllIterationsInterval;
EXPECT_EQ(kIterations, loop.Iterate(next_time));
last_time = next_time;
@@ -238,6 +255,43 @@
}
// Tests that the phased loop is correctly adjusting when the offset is
+// incremented multiple times.
+TEST_F(PhasedLoopTest, IncrementingOffset) {
+ constexpr int kCount = 5;
+ constexpr int kIterations = 10;
+ const auto kOffset = milliseconds(0);
+ const auto kInterval = milliseconds(1000);
+ const auto kAllIterationsInterval = kInterval * kIterations;
+
+ PhasedLoop loop(kInterval, monotonic_clock::epoch(), kOffset);
+ auto last_time = monotonic_clock::epoch() + kOffset + (kInterval * 3);
+ ASSERT_EQ(4, loop.Iterate(last_time));
+ for (int i = 1; i < kCount; i++) {
+ const auto offset = kOffset + milliseconds(i);
+ {
+ const monotonic_clock::time_point original_time = loop.sleep_time();
+ loop.set_interval_and_offset(kInterval, offset);
+ EXPECT_EQ(original_time - kInterval + milliseconds(1), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(last_time));
+ }
+ // Now, explicitly update/clear things to a set time. We add a milliseconds
+ // so that when we call Iterate() next we actually get the expected number
+ // of iterations (otherwise, there is an iteration that would happen at
+ // last_time + 1 that gets counted, which is correct behavior, and so just
+ // needs to be accounted for somehow).
+ {
+ loop.set_interval_and_offset(kInterval, offset,
+ last_time + milliseconds(1));
+ EXPECT_EQ(0, loop.Iterate(last_time + milliseconds(1)));
+ }
+
+ const auto next_time = last_time + milliseconds(1) + kAllIterationsInterval;
+ EXPECT_EQ(kIterations, loop.Iterate(next_time));
+ last_time = next_time;
+ }
+}
+
+// Tests that the phased loop is correctly adjusting when the offset is
// changed to 0.
TEST_F(PhasedLoopTest, ChangingOffset) {
const auto kOffset = milliseconds(900);
diff --git a/aos/util/threaded_consumer.h b/aos/util/threaded_consumer.h
new file mode 100644
index 0000000..95ec79a
--- /dev/null
+++ b/aos/util/threaded_consumer.h
@@ -0,0 +1,102 @@
+#ifndef AOS_UTIL_THREADED_CONSUMER_H_
+#define AOS_UTIL_THREADED_CONSUMER_H_
+
+#include <functional>
+#include <optional>
+#include <thread>
+
+#include "aos/condition.h"
+#include "aos/containers/ring_buffer.h"
+#include "aos/mutex/mutex.h"
+#include "aos/realtime.h"
+
+namespace aos {
+namespace util {
+
+// This class implements a threadpool of a single worker that accepts work
+// from the main thread through a queue and executes it at a different realtime
+// priority.
+//
+// There is no mechanism to get data back to the main thread, the worker only
+// acts as a consumer. When this class is destroyed, it join()s the worker and
+// finishes all outstanding tasks.
+template <typename T, int QueueSize>
+class ThreadedConsumer {
+ public:
+ // Constructs a new ThreadedConsumer with the given consumer function to be
+ // run at the given realtime priority. If worker_priority is zero, the thread
+ // will stay at non realtime priority.
+ ThreadedConsumer(std::function<void(T)> consumer_function,
+ int worker_priority)
+ : consumer_function_(consumer_function),
+ worker_priority_(worker_priority),
+ more_tasks_(&mutex_),
+ worker_thread_([this]() { WorkerFunction(); }) {}
+
+ ~ThreadedConsumer() {
+ {
+ aos::MutexLocker locker(&mutex_);
+ quit_ = true;
+ more_tasks_.Broadcast();
+ }
+ worker_thread_.join();
+ }
+
+ // Submits another task to be processed by the worker.
+ // Returns true if successfully pushed onto the queue, and false if the queue
+ // is full.
+ bool Push(T task) {
+ aos::MutexLocker locker(&mutex_);
+
+ if (task_queue_.full()) {
+ return false;
+ }
+
+ task_queue_.Push(task);
+ more_tasks_.Broadcast();
+
+ return true;
+ }
+
+ private:
+ void WorkerFunction() {
+ if (worker_priority_ > 0) {
+ aos::SetCurrentThreadRealtimePriority(worker_priority_);
+ }
+
+ while (true) {
+ std::optional<T> task;
+
+ {
+ aos::MutexLocker locker(&mutex_);
+ while (task_queue_.empty() && !quit_) {
+ CHECK(!more_tasks_.Wait());
+ }
+
+ if (task_queue_.empty() && quit_) break;
+
+ // Pop
+ task = std::move(task_queue_[0]);
+ task_queue_.Shift();
+ }
+
+ consumer_function_(*task);
+ task.reset();
+ }
+
+ aos::UnsetCurrentThreadRealtimePriority();
+ }
+
+ std::function<void(T)> consumer_function_;
+ aos::RingBuffer<T, QueueSize> task_queue_;
+ aos::Mutex mutex_;
+ bool quit_ = false;
+ int worker_priority_;
+ aos::Condition more_tasks_;
+ std::thread worker_thread_;
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_THREADWORKER_H_
diff --git a/aos/util/threaded_consumer_test.cc b/aos/util/threaded_consumer_test.cc
new file mode 100644
index 0000000..f137108
--- /dev/null
+++ b/aos/util/threaded_consumer_test.cc
@@ -0,0 +1,144 @@
+#include "aos/util/threaded_consumer.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace util {
+
+// We expect it to be able to pass through everything we submit and recieves it
+// in the order that we submitted it. It should also be able to take in more
+// tasks than the size of the ring buffer as long as the worker doesn't get
+// behind.
+TEST(ThreadedConsumerTest, BasicFunction) {
+ std::atomic<int> counter{0};
+
+ ThreadedConsumer<int, 4> threaded_consumer(
+ [&counter](int task) {
+ LOG(INFO) << "task:" << task << " counter: " << counter;
+ counter = task;
+ },
+ 0);
+
+ for (int number : {9, 7, 1, 3, 100, 300, 42}) {
+ EXPECT_TRUE(threaded_consumer.Push(number));
+
+ // wait
+ while (counter != number) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+
+ EXPECT_EQ(counter, number);
+ }
+}
+
+// We should be able to raise the realtime priority of the worker thread, and
+// everything should work the same. It should also reset back to lower priority
+// when shutting down the worker thread.
+TEST(ThreadedConsumerTest, ElevatedPriority) {
+ std::atomic<int> counter{0};
+
+ {
+ ThreadedConsumer<int, 4> threaded_consumer(
+ [&counter](int task) {
+ CheckRealtime();
+ LOG(INFO) << "task:" << task << " counter: " << counter;
+ counter = task;
+ },
+ 20);
+
+ for (int number : {9, 7, 1, 3, 100, 300, 42}) {
+ EXPECT_TRUE(threaded_consumer.Push(number));
+
+ // wait
+ while (counter != number) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+
+ EXPECT_EQ(counter, number);
+ }
+ }
+ // TODO: Check that the worker thread's priority actually gets reset before
+ // the thread is destroyed.
+
+ CheckNotRealtime();
+}
+
+// If the worker gets behind, we shouldn't silently take in more tasks and
+// destroy old ones.
+TEST(ThreadedConsumerTest, OverflowRingBuffer) {
+ std::atomic<int> counter{0};
+ std::atomic<int> should_block{true};
+
+ ThreadedConsumer<int, 4> threaded_consumer(
+ [&counter, &should_block](int task) {
+ LOG(INFO) << "task:" << task << " counter: " << counter;
+
+ counter = task;
+
+ // prevent it from making any progress to simulate it getting behind
+ while (should_block) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+ },
+ 20);
+
+ // It consumes the 5 and then our worker blocks.
+ EXPECT_TRUE(threaded_consumer.Push(5));
+
+ // Wait for it to consume 5
+ while (counter != 5) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ };
+
+ // 4 more fills up the queue.
+ for (int number : {8, 9, 7, 1}) {
+ EXPECT_TRUE(threaded_consumer.Push(number));
+ }
+
+ // this one should overflow the buffer.
+ EXPECT_FALSE(threaded_consumer.Push(101));
+
+ // clean up, so we don't join() an infinite loop
+ should_block = false;
+}
+
+// The class should destruct gracefully and finish all of its work before
+// dissapearing.
+TEST(ThreadedConsumerTest, FinishesTasksOnQuit) {
+ std::atomic<int> counter{0};
+ std::atomic<int> should_block{true};
+
+ {
+ ThreadedConsumer<int, 4> threaded_consumer(
+ [&counter, &should_block](int task) {
+ LOG(INFO) << "task:" << task << " counter: " << counter;
+
+ counter = task;
+
+ // prevent it from making any progress to simulate it getting behind
+ while (should_block) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+ },
+ 20);
+
+ // Give it some work to do
+ for (int number : {8, 9, 7, 1}) {
+ EXPECT_TRUE(threaded_consumer.Push(number));
+ }
+
+ // Wait for it to consume the first number
+ while (counter != 8) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ };
+
+ // allow it to continue
+ should_block = false;
+ }
+
+ // It should have finished all the work and gotten to the last number.
+ EXPECT_EQ(counter, 1);
+}
+
+} // namespace util
+} // namespace aos
diff --git a/debian/aws_sdk.BUILD b/debian/aws_sdk.BUILD
index a77f5bb..46ac375 100644
--- a/debian/aws_sdk.BUILD
+++ b/debian/aws_sdk.BUILD
@@ -50,9 +50,9 @@
"aws-cpp-sdk-core/include/aws/core/utils/crypto/openssl/*.h",
]),
copts = [
- "-DAWS_SDK_VERSION_MAJOR=19",
- "-DAWS_SDK_VERSION_MINOR=0",
- "-DAWS_SDK_VERSION_PATCH=\"\\\"0-RC1\"\\\"",
+ "-DAWS_SDK_VERSION_MAJOR=10",
+ "-DAWS_SDK_VERSION_MINOR=34",
+ "-DAWS_SDK_VERSION_PATCH=\"\\\"BRT\"\\\"",
"-DENABLE_OPENSSL_ENCRYPTION",
"-DENABLE_CURL_CLIENT",
"-Wno-cast-align",
@@ -75,7 +75,7 @@
genrule(
name = "gen_Config",
outs = ["crt/aws-crt-cpp/include/aws/crt/Config.h"],
- cmd = "echo '#define AWS_CRT_CPP_VERSION \"19.0.0-RC1\"' > $@",
+ cmd = "echo '#define AWS_CRT_CPP_VERSION \"1.10.34\"' > $@",
target_compatible_with = ["@platforms//os:linux"],
)
@@ -88,15 +88,19 @@
copts = [
"-Wno-sign-compare",
"-Wno-cast-qual",
+ "-Wno-tautological-type-limit-compare",
+ "-Wno-missing-field-initializers",
],
includes = ["crt/aws-crt-cpp/include"],
target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
deps = [
":aws-c-auth",
":aws-c-common",
":aws-c-event-stream",
":aws-c-mqtt",
":aws-c-s3",
+ ":aws-c-sdkutils",
],
)
@@ -109,6 +113,7 @@
"#define AWS_HAVE_GCC_INLINE_ASM 1",
"#undef AWS_HAVE_MSVC_MULX",
"#define AWS_HAVE_EXECINFO 1",
+ "#define AWS_AFFINITY_METHOD 0",
"END",
]),
target_compatible_with = ["@platforms//os:linux"],
@@ -118,6 +123,7 @@
name = "aws-c-common",
srcs = glob([
"crt/aws-crt-cpp/crt/aws-c-common/source/*.c",
+ "crt/aws-crt-cpp/crt/aws-c-common/source/external/*.c",
"crt/aws-crt-cpp/crt/aws-c-common/source/posix/*.c",
]) + [
":gen_config",
@@ -156,6 +162,7 @@
includes = ["crt/aws-crt-cpp/crt/aws-c-common/include"],
target_compatible_with = ["@platforms//os:linux"],
textual_hdrs = glob(["crt/aws-crt-cpp/crt/aws-c-common/include/**/*.inl"]),
+ visibility = ["//visibility:public"],
)
# -march=armv8-a+crc
@@ -209,8 +216,10 @@
]),
hdrs = glob(["crt/aws-crt-cpp/crt/aws-c-cal/include/**/*.h"]),
copts = [
+ "-DOPENSSL_IS_AWSLC",
"-Wno-incompatible-pointer-types",
"-Wno-unused-function",
+ "-Wno-unused-parameter",
"-Wno-cast-align",
"-Wno-cast-qual",
],
@@ -235,6 +244,7 @@
deps = [
":aws-c-auth",
":aws-c-common",
+ ":aws-checksums",
],
)
@@ -242,6 +252,7 @@
name = "aws-c-compression",
srcs = glob(["crt/aws-crt-cpp/crt/aws-c-compression/source/*.c"]),
hdrs = glob(["crt/aws-crt-cpp/crt/aws-c-compression/include/**/*.h"]),
+ copts = ["-Wno-cast-qual"],
includes = ["crt/aws-crt-cpp/crt/aws-c-compression/include"],
target_compatible_with = ["@platforms//os:linux"],
deps = [
@@ -269,6 +280,21 @@
)
cc_library(
+ name = "aws-c-sdkutils",
+ srcs = glob(["crt/aws-crt-cpp/crt/aws-c-sdkutils/source/**/*.c"]),
+ hdrs = glob(["crt/aws-crt-cpp/crt/aws-c-sdkutils/include/**/*.h"]),
+ copts = [
+ "-Wno-cast-align",
+ "-Wno-cast-qual",
+ ],
+ includes = ["crt/aws-crt-cpp/crt/aws-c-sdkutils/include"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":aws-c-common",
+ ],
+)
+
+cc_library(
name = "aws-c-auth",
srcs = glob(["crt/aws-crt-cpp/crt/aws-c-auth/source/**/*.c"]),
hdrs = glob(["crt/aws-crt-cpp/crt/aws-c-auth/include/**/*.h"]),
@@ -282,6 +308,7 @@
":aws-c-common",
":aws-c-http",
":aws-c-io",
+ ":aws-c-sdkutils",
],
)
@@ -292,11 +319,13 @@
copts = [
"-Wno-cast-qual",
"-Wno-cast-align",
+ "-DAWS_MQTT_WITH_WEBSOCKETS",
],
includes = ["crt/aws-crt-cpp/crt/aws-c-mqtt/include"],
target_compatible_with = ["@platforms//os:linux"],
deps = [
":aws-c-common",
+ ":aws-c-http",
":aws-c-io",
],
)
@@ -309,8 +338,13 @@
"crt/aws-crt-cpp/crt/aws-c-io/source/s2n/*.c",
"crt/aws-crt-cpp/crt/aws-c-io/source/posix/*.c",
]),
- hdrs = glob(["crt/aws-crt-cpp/crt/aws-c-io/include/**/*.h"]),
+ hdrs = glob(["crt/aws-crt-cpp/crt/aws-c-io/include/**/*.h"]) + [
+ "crt/aws-crt-cpp/crt/aws-c-io/source/pkcs11_private.h",
+ ] + glob([
+ "crt/aws-crt-cpp/crt/aws-c-io/source/pkcs11/v2.40/*.h",
+ ]),
copts = [
+ "-DUSE_S2N",
"-DAWS_USE_EPOLL",
"-Wno-cast-align",
"-Wno-cast-qual",
@@ -335,6 +369,7 @@
"crt/aws-crt-cpp/crt/s2n/utils/**/*.c",
"crt/aws-crt-cpp/crt/s2n/stuffer/**/*.c",
"crt/aws-crt-cpp/crt/s2n/crypto/**/*.c",
+ "crt/aws-crt-cpp/crt/s2n/pq-crypto/*.c",
]),
hdrs = ["crt/aws-crt-cpp/crt/s2n/api/s2n.h"],
copts = [
diff --git a/debian/aws_sdk.patch b/debian/aws_sdk.patch
index df613f5..8780e9b 100644
--- a/debian/aws_sdk.patch
+++ b/debian/aws_sdk.patch
@@ -4,89 +4,22 @@
index 761455b..fc434ba 100644
--- a/crt/aws-crt-cpp/crt/aws-c-cal/source/unix/openssl_platform_init.c
+++ b/crt/aws-crt-cpp/crt/aws-c-cal/source/unix/openssl_platform_init.c
-@@ -34,7 +34,7 @@ struct openssl_evp_md_ctx_table *g_aws_openssl_evp_md_ctx_table = NULL;
+@@ -37,7 +37,7 @@ struct openssl_evp_md_ctx_table *g_aws_openssl_evp_md_ctx_table = NULL;
/* 1.1 */
- extern HMAC_CTX *HMAC_CTX_new(void) __attribute__((weak)) __attribute__((used));
- extern void HMAC_CTX_free(HMAC_CTX *) __attribute__((weak)) __attribute__((used));
--extern int HMAC_CTX_reset(HMAC_CTX *) __attribute__((weak)) __attribute__((used));
-+//extern int HMAC_CTX_reset(HMAC_CTX *) __attribute__((weak)) __attribute__((used));
-
+ extern HMAC_CTX *HMAC_CTX_new(void) __attribute__((weak, used));
+ extern void HMAC_CTX_free(HMAC_CTX *) __attribute__((weak, used));
+-extern int HMAC_CTX_reset(HMAC_CTX *) __attribute__((weak, used));
++//extern int HMAC_CTX_reset(HMAC_CTX *) __attribute__((weak, used));
+
/* 1.0.2 */
- extern void HMAC_CTX_init(HMAC_CTX *) __attribute__((weak)) __attribute__((used));
-@@ -43,8 +43,8 @@ extern void HMAC_CTX_cleanup(HMAC_CTX *) __attribute__((weak)) __attribute__((us
+ extern void HMAC_CTX_init(HMAC_CTX *) __attribute__((weak, used));
+@@ -46,7 +46,7 @@ extern void HMAC_CTX_cleanup(HMAC_CTX *) __attribute__((weak)) __attribute__((us
/* common */
- extern int HMAC_Update(HMAC_CTX *, const unsigned char *, size_t) __attribute__((weak)) __attribute__((used));
- extern int HMAC_Final(HMAC_CTX *, unsigned char *, unsigned int *) __attribute__((weak)) __attribute__((used));
--extern int HMAC_Init_ex(HMAC_CTX *, const void *, int, const EVP_MD *, ENGINE *) __attribute__((weak))
--__attribute__((used));
-+//extern int HMAC_Init_ex(HMAC_CTX *, const void *, int, const EVP_MD *, ENGINE *) __attribute__((weak))
-+//__attribute__((used));
-
+ extern int HMAC_Update(HMAC_CTX *, const unsigned char *, size_t) __attribute__((weak, used));
+ extern int HMAC_Final(HMAC_CTX *, unsigned char *, unsigned int *) __attribute__((weak, used));
+-extern int HMAC_Init_ex(HMAC_CTX *, const void *, int, const EVP_MD *, ENGINE *) __attribute__((weak, used));
++//extern int HMAC_Init_ex(HMAC_CTX *, const void *, int, const EVP_MD *, ENGINE *) __attribute__((weak, used));
+
/* libcrypto 1.1 stub for init */
static void s_hmac_ctx_init_noop(HMAC_CTX *ctx) {
-@@ -393,9 +393,9 @@ void aws_cal_platform_init(struct aws_allocator *allocator) {
- }
- }
-
-- if (!CRYPTO_get_id_callback()) {
-- CRYPTO_set_id_callback(s_id_fn);
-- }
-+ //if (!CRYPTO_get_id_callback()) {
-+ //CRYPTO_set_id_callback(s_id_fn);
-+ //}
- }
-
- void aws_cal_platform_clean_up(void) {
-@@ -408,9 +408,9 @@ void aws_cal_platform_clean_up(void) {
- aws_mem_release(s_libcrypto_allocator, s_libcrypto_locks);
- }
-
-- if (CRYPTO_get_id_callback() == s_id_fn) {
-- CRYPTO_set_id_callback(NULL);
-- }
-+ //if (CRYPTO_get_id_callback() == s_id_fn) {
-+ //CRYPTO_set_id_callback(NULL);
-+ //}
- }
- #if !defined(__GNUC__) || (__GNUC__ >= 4 && __GNUC_MINOR__ > 1)
- # pragma GCC diagnostic pop
Submodule crt/s2n contains modified content
-diff --git a/crt/aws-crt-cpp/crt/s2n/utils/s2n_asn1_time.c b/crt/aws-crt-cpp/crt/s2n/utils/s2n_asn1_time.c
-index 84dbc6df..d3566b81 100755
---- a/crt/aws-crt-cpp/crt/s2n/utils/s2n_asn1_time.c
-+++ b/crt/aws-crt-cpp/crt/s2n/utils/s2n_asn1_time.c
-@@ -46,7 +46,7 @@ typedef enum parser_state {
- } parser_state;
-
- static inline long get_gmt_offset(struct tm *t) {
--#if defined(__FreeBSD__) || defined(__NetBSD__) || defined(__ANDROID__) || defined(ANDROID) || defined(__APPLE__) && defined(__MACH__)
-+#if defined(__FreeBSD__) || defined(__NetBSD__) || defined(__ANDROID__) || defined(ANDROID) || defined(__APPLE__) && defined(__MACH__) || defined(__USE_MISC)
- return t->tm_gmtoff;
- #else
- return t->__tm_gmtoff;
-diff --git a/crt/aws-crt-cpp/crt/s2n/utils/s2n_init.c b/crt/aws-crt-cpp/crt/s2n/utils/s2n_init.c
-index 0f79f959..ae8122fb 100644
---- a/crt/aws-crt-cpp/crt/s2n/utils/s2n_init.c
-+++ b/crt/aws-crt-cpp/crt/s2n/utils/s2n_init.c
-@@ -45,7 +45,7 @@ int s2n_init(void)
- GUARD_POSIX(s2n_security_policies_init());
- GUARD_POSIX(s2n_config_defaults_init());
- GUARD_POSIX(s2n_extension_type_init());
-- GUARD_AS_POSIX(s2n_pq_init());
-+ //GUARD_AS_POSIX(s2n_pq_init());
-
- S2N_ERROR_IF(atexit(s2n_cleanup_atexit) != 0, S2N_ERR_ATEXIT);
-
-diff --git a/crt/aws-crt-cpp/crt/aws-c-common/include/aws/common/private/lookup3.inl b/crt/aws-crt-cpp/crt/aws-c-common/include/aws/common/private/lookup3.inl
-index 0f79f959..ae8122fb 100644
---- a/crt/aws-crt-cpp/crt/aws-c-common/include/aws/common/private/lookup3.inl
-+++ b/crt/aws-crt-cpp/crt/aws-c-common/include/aws/common/private/lookup3.inl
-@@ -533,7 +533,7 @@
- * "CPROVER check pop". The masking trick does make the hash noticably
- * faster for short strings (like English words).
- */
--#ifndef VALGRIND
-+#if !defined(VALGRIND) && !__has_feature(address_sanitizer) && !__has_feature(memory_sanitizer)
- #ifdef CBMC
- # pragma CPROVER check push
- # pragma CPROVER check disable "pointer"
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index fcbed76..89b2182 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -135,6 +135,7 @@
kLongitudinalAccel = 2,
kLateralAccel = 3,
};
+
static constexpr int kNInputs = 4;
// Number of previous samples to save.
static constexpr int kSaveSamples = 200;
@@ -177,6 +178,135 @@
// State contains the states defined by the StateIdx enum. See comments there.
typedef Eigen::Matrix<Scalar, kNStates, 1> State;
+ // The following classes exist to allow us to support doing corections in the
+ // past by rewinding the EKF, calling the appropriate H and dhdx functions,
+ // and then playing everything back. Originally, this simply used
+ // std::function's, but doing so causes us to perform dynamic memory
+ // allocation in the core of the drivetrain control loop.
+ //
+ // The ExpectedObservationFunctor class serves to provide an interface for the
+ // actual H and dH/dX that the EKF itself needs. Most implementations end up
+ // just using this; in the degenerate case, ExpectedObservationFunctor could
+ // be implemented as a class that simply stores two std::functions and calls
+ // them when H() and DHDX() are called.
+ //
+ // The ObserveDeletion() and deleted() methods exist for sanity checking--we
+ // don't rely on them to do any work, but in order to ensure that memory is
+ // being managed correctly, we have the HybridEkf call ObserveDeletion() when
+ // it no longer needs an instance of the object.
+ class ExpectedObservationFunctor {
+ public:
+ virtual ~ExpectedObservationFunctor() = default;
+ // Return the expected measurement of the system for a given state and plant
+ // input.
+ virtual Output H(const State &state, const Input &input) = 0;
+ // Return the derivative of H() with respect to the state, given the current
+ // state.
+ virtual Eigen::Matrix<Scalar, kNOutputs, kNStates> DHDX(
+ const State &state) = 0;
+ virtual void ObserveDeletion() {
+ CHECK(!deleted_);
+ deleted_ = true;
+ }
+ bool deleted() const { return deleted_; }
+
+ private:
+ bool deleted_ = false;
+ };
+
+ // The ExpectedObservationBuilder creates a new ExpectedObservationFunctor.
+ // This is used for situations where in order to know what the correction
+ // methods even are we need to know the state at some time in the past. This
+ // is only used in the y2019 code and we've generally stopped using this
+ // pattern.
+ class ExpectedObservationBuilder {
+ public:
+ virtual ~ExpectedObservationBuilder() = default;
+ // The lifetime of the returned object should last at least until
+ // ObserveDeletion() is called on said object.
+ virtual ExpectedObservationFunctor *MakeExpectedObservations(
+ const State &state, const StateSquare &P) = 0;
+ void ObserveDeletion() {
+ CHECK(!deleted_);
+ deleted_ = true;
+ }
+ bool deleted() const { return deleted_; }
+
+ private:
+ bool deleted_ = false;
+ };
+
+ // The ExpectedObservationAllocator provides a utility class which manages the
+ // memory for a single type of correction step for a given localizer.
+ // Using the knowledge that at most kSaveSamples ExpectedObservation* objects
+ // can be referenced by the HybridEkf at any given time, this keeps an
+ // internal queue that more than mirrors the HybridEkf's internal queue, using
+ // the oldest spots in the queue to construct new ExpectedObservation*'s.
+ // This can be used with T as either a ExpectedObservationBuilder or
+ // ExpectedObservationFunctor. The appropriate Correct function will then be
+ // called in place of calling HybridEkf::Correct directly. Note that unless T
+ // implements both the Builder and Functor (which is generally discouraged),
+ // only one of the Correct* functions will build.
+ template <typename T>
+ class ExpectedObservationAllocator {
+ public:
+ ExpectedObservationAllocator(HybridEkf *ekf) : ekf_(ekf) {}
+ void CorrectKnownH(const Output &z, const Input *U, T H,
+ const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ aos::monotonic_clock::time_point t) {
+ if (functors_.full()) {
+ CHECK(functors_.begin()->functor->deleted());
+ }
+ auto pushed = functors_.PushFromBottom(Pair{t, std::move(H)});
+ if (pushed == functors_.end()) {
+ VLOG(1) << "Observation dropped off bottom of queue.";
+ return;
+ }
+ ekf_->Correct(z, U, nullptr, &pushed->functor.value(), R, t);
+ }
+ void CorrectKnownHBuilder(
+ const Output &z, const Input *U, T builder,
+ const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ aos::monotonic_clock::time_point t) {
+ if (functors_.full()) {
+ CHECK(functors_.begin()->functor->deleted());
+ }
+ auto pushed = functors_.PushFromBottom(Pair{t, std::move(builder)});
+ if (pushed == functors_.end()) {
+ VLOG(1) << "Observation dropped off bottom of queue.";
+ return;
+ }
+ ekf_->Correct(z, U, &pushed->functor.value(), nullptr, R, t);
+ }
+
+ private:
+ struct Pair {
+ aos::monotonic_clock::time_point t;
+ std::optional<T> functor;
+ friend bool operator<(const Pair &l, const Pair &r) { return l.t < r.t; }
+ };
+
+ HybridEkf *const ekf_;
+ aos::PriorityQueue<Pair, kSaveSamples + 1, std::less<Pair>> functors_;
+ };
+
+ // A simple implementation of ExpectedObservationFunctor for an LTI correction
+ // step. Does not store any external references, so overrides
+ // ObserveDeletion() to do nothing.
+ class LinearH : public ExpectedObservationFunctor {
+ public:
+ LinearH(const Eigen::Matrix<Scalar, kNOutputs, kNStates> &H) : H_(H) {}
+ virtual ~LinearH() = default;
+ Output H(const State &state, const Input &) final { return H_ * state; }
+ Eigen::Matrix<Scalar, kNOutputs, kNStates> DHDX(const State &) final {
+ return H_;
+ }
+ void ObserveDeletion() {}
+
+ private:
+ const Eigen::Matrix<Scalar, kNOutputs, kNStates> H_;
+ };
+
// Constructs a HybridEkf for a particular drivetrain.
// Currently, we use the drivetrain config for modelling constants
// (continuous time A and B matrices) and for the noise matrices for the
@@ -207,11 +337,8 @@
P_,
Input::Zero(),
Output::Zero(),
- {},
- [](const State &, const Input &) { return Output::Zero(); },
- [](const State &) {
- return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero();
- },
+ nullptr,
+ &H_encoders_and_gyro_.value(),
Eigen::Matrix<Scalar, kNOutputs, kNOutputs>::Identity(),
StateSquare::Identity(),
StateSquare::Zero(),
@@ -230,18 +357,11 @@
// on an assumption that the voltage was held constant between the time steps.
// TODO(james): Is it necessary to explicitly to provide a version with H as a
// matrix for linear cases?
- void Correct(
- const Output &z, const Input *U,
- std::function<void(const State &, const StateSquare &,
- std::function<Output(const State &, const Input &)> *,
- std::function<Eigen::Matrix<
- Scalar, kNOutputs, kNStates>(const State &)> *)>
- make_h,
- std::function<Output(const State &, const Input &)> h,
- std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
- dhdx,
- const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
- aos::monotonic_clock::time_point t);
+ void Correct(const Output &z, const Input *U,
+ ExpectedObservationBuilder *observation_builder,
+ ExpectedObservationFunctor *expected_observations,
+ const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ aos::monotonic_clock::time_point t);
// A utility function for specifically updating with encoder and gyro
// measurements.
@@ -291,11 +411,8 @@
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
R.setZero();
R.diagonal() << encoder_noise_, encoder_noise_, gyro_noise_;
- Correct(z, &U, {},
- [this](const State &X, const Input &) {
- return H_encoders_and_gyro_ * X;
- },
- [this](const State &) { return H_encoders_and_gyro_; }, R, t);
+ CHECK(H_encoders_and_gyro_.has_value());
+ Correct(z, &U, nullptr, &H_encoders_and_gyro_.value(), R, t);
}
// Sundry accessor:
@@ -348,6 +465,50 @@
private:
struct Observation {
+ Observation(aos::monotonic_clock::time_point t,
+ aos::monotonic_clock::time_point prev_t, State X_hat,
+ StateSquare P, Input U, Output z,
+ ExpectedObservationBuilder *make_h,
+ ExpectedObservationFunctor *h,
+ Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R, StateSquare A_d,
+ StateSquare Q_d,
+ aos::monotonic_clock::duration discretization_time,
+ State predict_update)
+ : t(t),
+ prev_t(prev_t),
+ X_hat(X_hat),
+ P(P),
+ U(U),
+ z(z),
+ make_h(make_h),
+ h(h),
+ R(R),
+ A_d(A_d),
+ Q_d(Q_d),
+ discretization_time(discretization_time),
+ predict_update(predict_update) {}
+ Observation(const Observation &) = delete;
+ Observation &operator=(const Observation &) = delete;
+ // Move-construct an observation by copying over the contents of the struct
+ // and then clearing the old Observation's pointers so that it doesn't try
+ // to clean things up.
+ Observation(Observation &&o)
+ : Observation(o.t, o.prev_t, o.X_hat, o.P, o.U, o.z, o.make_h, o.h, o.R,
+ o.A_d, o.Q_d, o.discretization_time, o.predict_update) {
+ o.make_h = nullptr;
+ o.h = nullptr;
+ }
+ Observation &operator=(Observation &&observation) = delete;
+ ~Observation() {
+ // Observe h being deleted first, since make_h may own its memory.
+ // Shouldn't actually matter, though.
+ if (h != nullptr) {
+ h->ObserveDeletion();
+ }
+ if (make_h != nullptr) {
+ make_h->ObserveDeletion();
+ }
+ }
// Time when the observation was taken.
aos::monotonic_clock::time_point t;
// Time that the previous observation was taken:
@@ -365,24 +526,11 @@
// estimate. This is used by the camera to make it so that we only have to
// match targets once.
// Only called if h and dhdx are empty.
- std::function<void(const State &, const StateSquare &,
- std::function<Output(const State &, const Input &)> *,
- std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)>
- make_h;
+ ExpectedObservationBuilder *make_h = nullptr;
// A function to calculate the expected output at a given state/input.
// TODO(james): For encoders/gyro, it is linear and the function call may
// be expensive. Potential source of optimization.
- std::function<Output(const State &, const Input &)> h;
- // The Jacobian of h with respect to x.
- // We assume that U has no impact on the Jacobian.
- // TODO(james): Currently, none of the users of this actually make use of
- // the ability to have dynamic dhdx (technically, the camera code should
- // recalculate it to be strictly correct, but I was both too lazy to do
- // so and it seemed unnecessary). This is a potential source for future
- // optimizations if function calls are being expensive.
- std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
- dhdx;
+ ExpectedObservationFunctor *h = nullptr;
// The measurement noise matrix.
Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
@@ -556,7 +704,7 @@
}
void CorrectImpl(Observation *obs, State *state, StateSquare *P) {
- const Eigen::Matrix<Scalar, kNOutputs, kNStates> H = obs->dhdx(*state);
+ const Eigen::Matrix<Scalar, kNOutputs, kNStates> H = obs->h->DHDX(*state);
// Note: Technically, this does calculate P * H.transpose() twice. However,
// when I was mucking around with some things, I found that in practice
// putting everything into one expression and letting Eigen optimize it
@@ -566,7 +714,7 @@
*P * H.transpose() * (H * *P * H.transpose() + obs->R).inverse();
const StateSquare Ptemp = (StateSquare::Identity() - K * H) * *P;
*P = Ptemp;
- *state += K * (obs->z - obs->h(*state, obs->U));
+ *state += K * (obs->z - obs->h->H(*state, obs->U));
}
void ProcessObservation(Observation *obs, const std::chrono::nanoseconds dt,
@@ -576,9 +724,9 @@
if (dt.count() != 0 && dt < kMaxTimestep) {
PredictImpl(obs, dt, state, P);
}
- if (!(obs->h && obs->dhdx)) {
- CHECK(obs->make_h);
- obs->make_h(*state, *P, &obs->h, &obs->dhdx);
+ if (obs->h == nullptr) {
+ CHECK(obs->make_h != nullptr);
+ obs->h = CHECK_NOTNULL(obs->make_h->MakeExpectedObservations(*state, *P));
}
CorrectImpl(obs, state, P);
}
@@ -590,7 +738,7 @@
StateSquare A_continuous_;
StateSquare Q_continuous_;
StateSquare P_;
- Eigen::Matrix<Scalar, kNOutputs, kNStates> H_encoders_and_gyro_;
+ std::optional<LinearH> H_encoders_and_gyro_;
Scalar encoder_noise_, gyro_noise_;
Eigen::Matrix<Scalar, kNStates, kNInputs> B_continuous_;
@@ -610,14 +758,8 @@
template <typename Scalar>
void HybridEkf<Scalar>::Correct(
const Output &z, const Input *U,
- std::function<void(const State &, const StateSquare &,
- std::function<Output(const State &, const Input &)> *,
- std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
- const State &)> *)>
- make_h,
- std::function<Output(const State &, const Input &)> h,
- std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
- dhdx,
+ ExpectedObservationBuilder *observation_builder,
+ ExpectedObservationFunctor *expected_observations,
const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
aos::monotonic_clock::time_point t) {
CHECK(!observations_.empty());
@@ -627,9 +769,9 @@
return;
}
auto cur_it = observations_.PushFromBottom(
- {t, t, State::Zero(), StateSquare::Zero(), Input::Zero(), z, make_h, h,
- dhdx, R, StateSquare::Identity(), StateSquare::Zero(),
- std::chrono::seconds(0), State::Zero()});
+ {t, t, State::Zero(), StateSquare::Zero(), Input::Zero(), z,
+ observation_builder, expected_observations, R, StateSquare::Identity(),
+ StateSquare::Zero(), std::chrono::seconds(0), State::Zero()});
if (cur_it == observations_.end()) {
VLOG(1) << "Camera dropped off of end with time of "
<< aos::time::DurationInSeconds(t.time_since_epoch())
@@ -773,14 +915,18 @@
std::pow(1.1, 2.0);
Q_continuous_(kLateralVelocity, kLateralVelocity) = std::pow(0.1, 2.0);
- H_encoders_and_gyro_.setZero();
- // Encoders are stored directly in the state matrix, so are a minor
- // transform away.
- H_encoders_and_gyro_(0, kLeftEncoder) = 1.0;
- H_encoders_and_gyro_(1, kRightEncoder) = 1.0;
- // Gyro rate is just the difference between right/left side speeds:
- H_encoders_and_gyro_(2, kLeftVelocity) = -1.0 / diameter;
- H_encoders_and_gyro_(2, kRightVelocity) = 1.0 / diameter;
+ {
+ Eigen::Matrix<Scalar, kNOutputs, kNStates> H_encoders_and_gyro;
+ H_encoders_and_gyro.setZero();
+ // Encoders are stored directly in the state matrix, so are a minor
+ // transform away.
+ H_encoders_and_gyro(0, kLeftEncoder) = 1.0;
+ H_encoders_and_gyro(1, kRightEncoder) = 1.0;
+ // Gyro rate is just the difference between right/left side speeds:
+ H_encoders_and_gyro(2, kLeftVelocity) = -1.0 / diameter;
+ H_encoders_and_gyro(2, kRightVelocity) = 1.0 / diameter;
+ H_encoders_and_gyro_.emplace(H_encoders_and_gyro);
+ }
encoder_noise_ = 5e-9;
gyro_noise_ = 1e-13;
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index 4bdf4de..5efb297 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -213,8 +213,7 @@
HybridEkf<>::Output Z(0.5, 0.5, 1);
Eigen::Matrix<double, 3, 12> H;
H.setIdentity();
- auto h = [H](const State &X, const Input &) { return H * X; };
- auto dhdx = [H](const State &) { return H; };
+ HybridEkf<>::LinearH h(H);
Eigen::Matrix<double, 3, 3> R;
R.setIdentity();
R *= 1e-3;
@@ -222,7 +221,7 @@
EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kTheta));
const double starting_p_norm = ekf_.P().norm();
for (int ii = 0; ii < 100; ++ii) {
- ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_);
+ ekf_.Correct(Z, &U, nullptr, &h, R, t0_);
}
EXPECT_NEAR(Z(0, 0), ekf_.X_hat(StateIdx::kX), 1e-3);
EXPECT_NEAR(Z(1, 0), ekf_.X_hat(StateIdx::kY), 1e-3);
@@ -241,8 +240,7 @@
State true_X = ekf_.X_hat();
Eigen::Matrix<double, 3, 12> H;
H.setZero();
- auto h = [H](const State &X, const Input &) { return H * X; };
- auto dhdx = [H](const State &) { return H; };
+ HybridEkf<>::LinearH h(H);
// Provide constant input voltage.
Input U;
U << 12.0, 10.0, 1.0, -0.1;
@@ -253,7 +251,7 @@
EXPECT_EQ(0.0, ekf_.X_hat().norm());
const double starting_p_norm = ekf_.P().norm();
for (int ii = 0; ii < 100; ++ii) {
- ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_ + dt_config_.dt * (ii + 1));
+ ekf_.Correct(Z, &U, nullptr, &h, R, t0_ + dt_config_.dt * (ii + 1));
true_X = Update(true_X, U, false);
EXPECT_EQ(true_X, ekf_.X_hat());
}
@@ -292,8 +290,7 @@
Z.setZero();
Eigen::Matrix<double, 3, 12> H;
H.setZero();
- auto h_zero = [H](const State &X, const Input &) { return H * X; };
- auto dhdx_zero = [H](const State &) { return H; };
+ HybridEkf<>::LinearH h_zero(H);
Input U;
U << 12.0, 12.0, 0.0, 0.0;
Eigen::Matrix<double, 3, 3> R;
@@ -302,8 +299,7 @@
// We fill up the buffer to be as full as demanded by the user.
const size_t n_predictions = GetParam();
for (size_t ii = 0; ii < n_predictions; ++ii) {
- ekf_.Correct(Z, &U, {}, h_zero, dhdx_zero, R,
- t0_ + dt_config_.dt * (ii + 1));
+ ekf_.Correct(Z, &U, nullptr, &h_zero, R, t0_ + dt_config_.dt * (ii + 1));
}
// Store state and covariance after prediction steps.
@@ -315,13 +311,12 @@
H(0, 0) = 1;
H(1, 1) = 1;
H(2, 2) = 1;
- auto h = [H](const State &X, const Input &) { return H * X; };
- auto dhdx = [H](const State &) { return H; };
+ HybridEkf<>::LinearH h(H);
R.setZero();
R.diagonal() << 1e-5, 1e-5, 1e-5;
U.setZero();
for (int ii = 0; ii < 20; ++ii) {
- ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_);
+ ekf_.Correct(Z, &U, nullptr, &h, R, t0_);
}
const double corrected_p_norm = ekf_.P().norm();
State expected_X_hat = modeled_X_hat;
@@ -348,8 +343,7 @@
Z.setZero();
Eigen::Matrix<double, 3, 12> H;
H.setZero();
- auto h_zero = [H](const State &X, const Input &) { return H * X; };
- auto dhdx_zero = [H](const State &) { return H; };
+ HybridEkf<>::LinearH h_zero(H);
Input U;
U << 12.0, 12.0, 0.0, 0.0;
Eigen::Matrix<double, 3, 3> R;
@@ -357,8 +351,7 @@
EXPECT_EQ(0.0, ekf_.X_hat().norm());
for (int ii = 0; ii < HybridEkf<>::kSaveSamples; ++ii) {
- ekf_.Correct(Z, &U, {}, h_zero, dhdx_zero, R,
- t0_ + dt_config_.dt * (ii + 1));
+ ekf_.Correct(Z, &U, nullptr, &h_zero, R, t0_ + dt_config_.dt * (ii + 1));
}
const State modeled_X_hat = ekf_.X_hat();
const HybridEkf<>::StateSquare modeled_P = ekf_.P();
@@ -368,12 +361,11 @@
H(0, 0) = 1;
H(1, 1) = 1;
H(2, 2) = 1;
- auto h = [H](const State &X, const Input &) { return H * X; };
- auto dhdx = [H](const State &) { return H; };
+ HybridEkf<>::LinearH h(H);
R.setIdentity();
R *= 1e-5;
U.setZero();
- ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_);
+ ekf_.Correct(Z, &U, nullptr, &h, R, t0_);
EXPECT_EQ(ekf_.X_hat(), modeled_X_hat)
<< "Expected too-old correction to have no effect; X_hat: "
<< ekf_.X_hat() << " expected " << modeled_X_hat;
@@ -493,8 +485,8 @@
// Expect death if the user does not provide U when creating a fresh
// measurement.
EXPECT_DEATH(
- ekf_.Correct({1, 2, 3}, nullptr, {}, {}, {}, Eigen::Matrix3d::Zero(),
- t0_ + std::chrono::seconds(1)),
+ ekf_.Correct({1, 2, 3}, nullptr, nullptr, nullptr,
+ Eigen::Matrix3d::Zero(), t0_ + std::chrono::seconds(1)),
"U != nullptr");
}
@@ -504,23 +496,10 @@
// Check that we die when no h-related functions are provided:
Input U;
U << 1.0, 2.0, 0.0, 0.0;
- EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {}, {}, Eigen::Matrix3d::Zero(),
- t0_ + std::chrono::seconds(1)),
- "make_h");
- // Check that we die when only one of h and dhdx are provided:
EXPECT_DEATH(
- ekf_.Correct(
- {1, 2, 3}, &U, {}, {},
- [](const State &) { return Eigen::Matrix<double, 3, 12>::Zero(); },
- Eigen::Matrix3d::Zero(), t0_ + std::chrono::seconds(1)),
+ ekf_.Correct({1, 2, 3}, &U, nullptr, nullptr, Eigen::Matrix3d::Zero(),
+ t0_ + std::chrono::seconds(1)),
"make_h");
- EXPECT_DEATH(ekf_.Correct(
- {1, 2, 3}, &U, {},
- [](const State &, const Input &) {
- return Eigen::Matrix<double, 3, 1>::Zero();
- },
- {}, Eigen::Matrix3d::Zero(), t0_ + std::chrono::seconds(1)),
- "make_h");
}
} // namespace testing
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
index 44e4f49..886a5d3 100755
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -7,7 +7,7 @@
from gi.repository import Gdk
from gi.repository import GdkX11
import cairo
-from constants import *
+from frc971.control_loops.python.constants import *
identity = cairo.Matrix()
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 80a4a53..2fed5f0 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -3,6 +3,7 @@
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
import numpy
+import math
import sys
from matplotlib import pylab
import glog
@@ -33,7 +34,8 @@
dt=0.00505,
controller_poles=[0.90, 0.90],
observer_poles=[0.02, 0.02],
- robot_cg_offset=0.0):
+ robot_cg_offset=0.0,
+ coefficient_of_friction=1.0):
"""Defines all constants of a drivetrain.
Args:
@@ -107,6 +109,7 @@
self.num_motors = num_motors
self.controller_poles = controller_poles
self.observer_poles = observer_poles
+ self.coefficient_of_friction = coefficient_of_friction
class Drivetrain(control_loop.ControlLoop):
@@ -533,6 +536,221 @@
kf_loop_writer.Write(kf_drivetrain_files[0], kf_drivetrain_files[1])
+def PlotDrivetrainSprint(drivetrain_params):
+ # Simulate the response of the system to a step input.
+ drivetrain = KFDrivetrain(left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
+ simulated_left_position = []
+ simulated_right_position = []
+ simulated_left_velocity = []
+ simulated_right_velocity = []
+
+ simulated_left_motor_currents = []
+ simulated_left_breaker_currents = []
+ simulated_right_motor_currents = []
+ simulated_right_breaker_currents = []
+
+ simulated_battery_heat_wattages = []
+ simulated_wattage = []
+ motor_inverter_voltages = []
+ voltage_left = []
+ voltage_right = []
+ simulated_motor_heat_wattages = []
+ simulated_motor_wattage = []
+
+ max_motor_currents = []
+ overall_currents = []
+ simulated_battery_wattage = []
+
+ # Distance in meters to call 1/2 field.
+ kSprintDistance = 8.0
+
+ vbat = 12.6
+ # Measured resistance of the battery, pd board, and breakers.
+ Rw = 0.023
+ top_speed = drivetrain.free_speed * (drivetrain.Gr +
+ drivetrain.Gl) / 2.0 * drivetrain.r
+
+ passed_distance = False
+ max_breaker_current = 0
+ heat_energy_usage = 0.0
+ for index in range(800):
+ # Current per side
+ left_traction_current = (drivetrain.mass / 2.0 *
+ drivetrain_params.coefficient_of_friction *
+ 9.81 * drivetrain.r * drivetrain.Gl /
+ drivetrain.Kt)
+ right_traction_current = (drivetrain.mass / 2.0 *
+ drivetrain_params.coefficient_of_friction *
+ 9.81 * drivetrain.r * drivetrain.Gr /
+ drivetrain.Kt)
+
+ # Detect if we've traveled over the sprint distance and report stats.
+ if (drivetrain.X[0, 0] + drivetrain.X[2, 0]) / 2.0 > kSprintDistance:
+ if not passed_distance:
+ velocity = (drivetrain.X[1, 0] + drivetrain.X[3, 0]) / 2.0
+ print("Took", index * drivetrain.dt,
+ "to pass 1/2 field, going", velocity, "m/s,",
+ velocity / 0.0254 / 12.0, "Traction limit current",
+ left_traction_current / drivetrain_params.num_motors,
+ "max breaker current", max_breaker_current, "top speed",
+ top_speed, "m/s", top_speed / 0.0254 / 12.0,
+ "fps, gear ratio", drivetrain.Gl, "heat energy",
+ heat_energy_usage)
+ passed_distance = True
+
+ bemf_left = drivetrain.X[
+ 1, 0] / drivetrain.r / drivetrain.Gl / drivetrain.Kv
+ bemf_right = drivetrain.X[
+ 3, 0] / drivetrain.r / drivetrain.Gr / drivetrain.Kv
+
+ # Max current we could push through the motors is what we would get if
+ # we short the battery through the battery resistance into the motor.
+ max_motor_current = (vbat - (bemf_left + bemf_right) / 2.0) / (
+ Rw + drivetrain.resistance / 2.0)
+
+ max_motor_currents.append(max_motor_current /
+ (drivetrain_params.num_motors * 2))
+
+ # From this current, we can compute the voltage we can apply.
+ # This is either the traction limit or the current limit.
+ max_voltage_left = bemf_left + min(
+ max_motor_current / 2,
+ left_traction_current) * drivetrain.resistance
+ max_voltage_right = bemf_right + min(
+ max_motor_current / 2,
+ right_traction_current) * drivetrain.resistance
+
+ simulated_left_position.append(drivetrain.X[0, 0])
+ simulated_left_velocity.append(drivetrain.X[1, 0])
+ simulated_right_position.append(drivetrain.X[2, 0])
+ simulated_right_velocity.append(drivetrain.X[3, 0])
+
+ U = numpy.matrix([[min(max_voltage_left, vbat)],
+ [min(max_voltage_right, vbat)]])
+
+ # Stator current
+ simulated_left_motor_current = (U[0, 0] -
+ bemf_left) / drivetrain.resistance
+ simulated_right_motor_current = (U[1, 0] -
+ bemf_right) / drivetrain.resistance
+
+ # And this gives us the power pushed into the motors.
+ power = (U[0, 0] * simulated_left_motor_current +
+ U[1, 0] * simulated_right_motor_current)
+
+ simulated_wattage.append(power)
+
+ # Solve for the voltage we'd have to supply to the input of the motor
+ # controller to generate the power required.
+ motor_inverter_voltage = (
+ vbat + numpy.sqrt(vbat * vbat - 4.0 * power * Rw)) / 2.0
+
+ overall_current = (vbat - motor_inverter_voltage) / Rw
+ overall_currents.append(overall_current)
+
+ motor_inverter_voltages.append(motor_inverter_voltage)
+
+ # Overall left and right currents at the breaker
+ simulated_left_breaker_current = (
+ simulated_left_motor_current /
+ drivetrain_params.num_motors) * U[0, 0] / motor_inverter_voltage
+ simulated_right_breaker_current = (
+ simulated_right_motor_current /
+ drivetrain_params.num_motors) * U[1, 0] / motor_inverter_voltage
+
+ simulated_left_motor_currents.append(simulated_left_motor_current /
+ drivetrain_params.num_motors)
+ simulated_left_breaker_currents.append(simulated_left_breaker_current)
+ simulated_right_motor_currents.append(simulated_right_motor_current /
+ drivetrain_params.num_motors)
+ simulated_right_breaker_currents.append(
+ simulated_right_breaker_current)
+
+ # Save out the peak battery current observed.
+ max_breaker_current = max(
+ max_breaker_current,
+ max(simulated_left_breaker_current,
+ simulated_right_breaker_current))
+
+ # Compute the heat burned in the battery
+ simulated_battery_heat_wattage = math.pow(
+ vbat - motor_inverter_voltage, 2.0) / Rw
+ simulated_battery_heat_wattages.append(simulated_battery_heat_wattage)
+
+ motor_heat_wattage = (math.pow(simulated_left_motor_current, 2.0) *
+ drivetrain.resistance +
+ math.pow(simulated_right_motor_current, 2.0) *
+ drivetrain.resistance)
+ simulated_motor_heat_wattages.append(motor_heat_wattage)
+
+ simulated_motor_wattage.append(simulated_left_motor_current * U[0, 0] +
+ simulated_right_motor_current * U[1, 0])
+
+ simulated_battery_wattage.append(vbat * overall_current)
+
+ # And then the overall energy outputted by the battery.
+ heat_energy_usage += (motor_heat_wattage +
+ simulated_battery_heat_wattage) * drivetrain.dt
+
+ voltage_left.append(U[0, 0])
+ voltage_right.append(U[1, 0])
+
+ drivetrain.Update(U)
+
+ t = [drivetrain.dt * x for x in range(len(simulated_left_position))]
+ pylab.rc('lines', linewidth=4)
+ pylab.subplot(3, 1, 1)
+ pylab.plot(t, simulated_left_position, label='left position')
+ pylab.plot(t, simulated_right_position, 'r--', label='right position')
+ pylab.plot(t, simulated_left_velocity, label='left velocity')
+ pylab.plot(t, simulated_right_velocity, label='right velocity')
+
+ pylab.suptitle('Acceleration Test\n12 Volt Step Input')
+ pylab.legend(loc='lower right')
+
+ pylab.subplot(3, 1, 2)
+
+ pylab.plot(t, simulated_left_motor_currents, label='left rotor current')
+ pylab.plot(t,
+ simulated_right_motor_currents,
+ 'r--',
+ label='right rotor current')
+ pylab.plot(t,
+ simulated_left_breaker_currents,
+ label='left breaker current')
+ pylab.plot(t,
+ simulated_right_breaker_currents,
+ 'r--',
+ label='right breaker current')
+ pylab.plot(t, motor_inverter_voltages, label='motor inverter voltage')
+ pylab.plot(t, voltage_left, label='left voltage')
+ pylab.plot(t, voltage_right, label='right voltage')
+ pylab.plot(t, max_motor_currents, label='max_currents')
+ pylab.legend(loc='lower right')
+
+ wattage_axis = pylab.subplot(3, 1, 3)
+ wattage_axis.plot(t, simulated_wattage, label='wattage')
+ wattage_axis.plot(t,
+ simulated_battery_heat_wattages,
+ label='battery wattage')
+ wattage_axis.plot(t,
+ simulated_motor_heat_wattages,
+ label='motor heat wattage')
+ wattage_axis.plot(t, simulated_motor_wattage, label='motor wattage')
+ wattage_axis.plot(t, simulated_battery_wattage, label='overall wattage')
+ pylab.legend(loc='upper left')
+ overall_current_axis = wattage_axis.twinx()
+ overall_current_axis.plot(t, overall_currents, 'c--', label='current')
+
+ pylab.legend(loc='lower right')
+
+ pylab.suptitle('Acceleration Test\n12 Volt Step Input\n%f fps' %
+ (top_speed / 0.0254 / 12.0, ))
+ pylab.show()
+
+
def PlotDrivetrainMotions(drivetrain_params):
# Test out the voltage error.
drivetrain = KFDrivetrain(left_low=False,
diff --git a/frc971/imu_reader/BUILD b/frc971/imu_reader/BUILD
new file mode 100644
index 0000000..7d424c4
--- /dev/null
+++ b/frc971/imu_reader/BUILD
@@ -0,0 +1,21 @@
+cc_library(
+ name = "imu",
+ srcs = [
+ "imu.cc",
+ ],
+ hdrs = [
+ "imu.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:epoll",
+ "//aos/events:shm_event_loop",
+ "//aos/util:crc32",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//frc971/wpilib:imu_fbs",
+ "//y2022:constants",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/types:span",
+ ],
+)
diff --git a/y2022/localizer/imu.cc b/frc971/imu_reader/imu.cc
similarity index 90%
rename from y2022/localizer/imu.cc
rename to frc971/imu_reader/imu.cc
index 2310e99..b477e4c 100644
--- a/y2022/localizer/imu.cc
+++ b/frc971/imu_reader/imu.cc
@@ -1,10 +1,9 @@
-#include "y2022/localizer/imu.h"
+#include "frc971/imu_reader/imu.h"
#include "aos/util/crc32.h"
#include "glog/logging.h"
-#include "y2022/constants.h"
-namespace y2022::localizer {
+namespace frc971::imu {
namespace {
@@ -15,13 +14,11 @@
} // namespace
-Imu::Imu(aos::ShmEventLoop *event_loop)
+Imu::Imu(aos::ShmEventLoop *event_loop, double encoder_scalar)
: event_loop_(event_loop),
imu_sender_(
- event_loop_->MakeSender<frc971::IMUValuesBatch>("/localizer")) {
- event_loop->SetRuntimeRealtimePriority(30);
- PCHECK(system("sudo chmod 644 /dev/adis16505") == 0)
- << ": Failed to set read permissions on IMU device.";
+ event_loop_->MakeSender<frc971::IMUValuesBatch>("/localizer")),
+ encoder_scalar_(encoder_scalar) {
imu_fd_ = open("/dev/adis16505", O_RDONLY | O_NONBLOCK);
PCHECK(imu_fd_ != -1) << ": Failed to open SPI device for IMU.";
aos::internal::EPoll *epoll = event_loop_->epoll();
@@ -123,10 +120,8 @@
// extra data from the pico
imu_builder.add_pico_timestamp_us(pico_timestamp);
- imu_builder.add_left_encoder(
- -constants::Values::DrivetrainEncoderToMeters(encoder2_count));
- imu_builder.add_right_encoder(
- constants::Values::DrivetrainEncoderToMeters(encoder1_count));
+ imu_builder.add_left_encoder(-encoder_scalar_ * encoder2_count);
+ imu_builder.add_right_encoder(encoder_scalar_ * encoder1_count);
imu_builder.add_previous_reading_diag_stat(diag_stat_offset);
}
@@ -167,4 +162,5 @@
}
Imu::~Imu() { PCHECK(0 == close(imu_fd_)); }
-} // namespace y2022::localizer
+
+} // namespace frc971::imu
diff --git a/y2022/localizer/imu.h b/frc971/imu_reader/imu.h
similarity index 71%
rename from y2022/localizer/imu.h
rename to frc971/imu_reader/imu.h
index cd45710..e3285d0 100644
--- a/y2022/localizer/imu.h
+++ b/frc971/imu_reader/imu.h
@@ -1,16 +1,17 @@
-#ifndef Y2022_LOCALIZER_IMU_H_
-#define Y2022_LOCALIZER_IMU_H_
+#ifndef FRC971_IMU_READER_IMU_H_
+#define FRC971_IMU_READER_IMU_H_
+
#include "aos/events/shm_event_loop.h"
#include "frc971/wpilib/imu_batch_generated.h"
-#include "y2022/constants.h"
-namespace y2022::localizer {
+namespace frc971::imu {
// Reads IMU packets from the kernel driver which reads them over spi
// from the Raspberry Pi Pico on the IMU board.
class Imu {
public:
- Imu(aos::ShmEventLoop *event_loop);
+ // Constructs an IMU reader object. encoder_scalar is in meters/count.
+ Imu(aos::ShmEventLoop *event_loop, double encoder_scalar);
~Imu();
private:
@@ -26,6 +27,10 @@
int imu_fd_;
uint failed_checksums_ = 0;
+
+ double encoder_scalar_;
};
-} // namespace y2022::localizer
-#endif // Y2022_LOCALIZER_IMU_H_
+
+} // namespace frc971::imu
+
+#endif // FRC971_IMU_READER_IMU_H_
diff --git a/frc971/rockpi/.config b/frc971/rockpi/.config
index 636a585..3860117 100644
--- a/frc971/rockpi/.config
+++ b/frc971/rockpi/.config
@@ -3358,6 +3358,7 @@
# Camera sensor devices
#
# CONFIG_VIDEO_AR0521 is not set
+CONFIG_VIDEO_ARDUCAM_PIVARIETY=m
# CONFIG_VIDEO_HI556 is not set
# CONFIG_VIDEO_HI846 is not set
# CONFIG_VIDEO_HI847 is not set
diff --git a/frc971/rockpi/build_rootfs.sh b/frc971/rockpi/build_rootfs.sh
index b6c8338..74d4dc1 100755
--- a/frc971/rockpi/build_rootfs.sh
+++ b/frc971/rockpi/build_rootfs.sh
@@ -9,9 +9,32 @@
KERNEL_VERSION=6.0.8-rt14-rockpi4b
PARTITION="${IMAGE}.partition"
-# TODO(austin): Make sure flex, bison, gcc-aarch64-linux-gnu,
-# gcc-arm-none-eabi, device-tree-compiler, swig are installed
-# rather than let the user figure this out on their own the hard way.
+# Check if dependencies are missing.
+missing_deps=()
+REQUIRED_DEPS=(
+ flex
+ bison
+ gcc-arm-none-eabi
+ gcc-aarch64-linux-gnu
+ device-tree-compiler
+ swig
+ debootstrap
+)
+for dep in "${REQUIRED_DEPS[@]}"; do
+ if ! dpkg-query -W -f='${Status}' "${dep}" | grep -q "install ok installed"; then
+ missing_deps+=("${dep}")
+ fi
+done
+
+# Print missing dependencies
+if ((${#missing_deps[@]} != 0 )); then
+ echo "Missing dependencies, please install:"
+ echo apt install "${missing_deps[@]}"
+ exit 1
+else
+ echo -e "\033[32mAll dependencies are already installed\033[0m"
+fi
+
export CC=aarch64-linux-gnu-
# Reset any existing mounts.
@@ -46,7 +69,7 @@
# Now, build uboot.
if [[ ! -e u-boot ]]; then
- git clone -b "${UBOOT_VERSION} https://github.com/u-boot/u-boot" --depth=1
+ git clone -b "${UBOOT_VERSION}" https://github.com/u-boot/u-boot --depth=1
fi
pushd u-boot/
@@ -106,13 +129,13 @@
# Run the string command as root inside the target.
function target() {
- sudo chroot --userspec=root.root "${PARTITION}" qemu-aarch64-static \
+ sudo chroot --userspec=root:root "${PARTITION}" qemu-aarch64-static \
/bin/bash -c "$1"
}
# Run the string command as the pi user inside the target.
function pi_target() {
- sudo chroot --userspec=pi.pi "${PARTITION}" qemu-aarch64-static \
+ sudo chroot --userspec=pi:pi --groups=pi "${PARTITION}" qemu-aarch64-static \
/bin/bash -c "$1"
}
@@ -153,10 +176,17 @@
copyfile root.root 644 etc/apt/sources.list.d/frc971.list
target "apt-get update"
-target "apt-get -y install -t bullseye-backports systemd"
+target "apt-get -y install -t bullseye-backports systemd systemd-resolved"
+
+# Make systemd-resolved work.
+target_mkdir root.root 755 run/systemd
+target_mkdir systemd-resolve.systemd-resolve 755 run/systemd/resolve
+copyfile systemd-resolve.systemd-resolve 644 run/systemd/resolve/stub-resolv.conf
+target "systemctl enable systemd-resolved"
+
target "apt-get -y install -t bullseye-backports bpfcc-tools"
-target "apt-get install -y sudo openssh-server python3 bash-completion git v4l-utils cpufrequtils pmount rsync vim-nox chrony libopencv-calib3d4.5 libopencv-contrib4.5 libopencv-core4.5 libopencv-features2d4.5 libopencv-flann4.5 libopencv-highgui4.5 libopencv-imgcodecs4.5 libopencv-imgproc4.5 libopencv-ml4.5 libopencv-objdetect4.5 libopencv-photo4.5 libopencv-shape4.5 libopencv-stitching4.5 libopencv-superres4.5 libopencv-video4.5 libopencv-videoio4.5 libopencv-videostab4.5 libopencv-viz4.5 libnice10 pmount libnice-dev feh libgstreamer1.0-0 libgstreamer-plugins-base1.0-0 libgstreamer-plugins-bad1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-nice usbutils locales trace-cmd clinfo"
+target "apt-get install -y sudo openssh-server python3 bash-completion git v4l-utils cpufrequtils pmount rsync vim-nox chrony libopencv-calib3d4.5 libopencv-contrib4.5 libopencv-core4.5 libopencv-features2d4.5 libopencv-flann4.5 libopencv-highgui4.5 libopencv-imgcodecs4.5 libopencv-imgproc4.5 libopencv-ml4.5 libopencv-objdetect4.5 libopencv-photo4.5 libopencv-shape4.5 libopencv-stitching4.5 libopencv-superres4.5 libopencv-video4.5 libopencv-videoio4.5 libopencv-videostab4.5 libopencv-viz4.5 libnice10 pmount libnice-dev feh libgstreamer1.0-0 libgstreamer-plugins-base1.0-0 libgstreamer-plugins-bad1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-nice usbutils locales trace-cmd clinfo jq"
target "cd /tmp && wget https://software.frc971.org/Build-Dependencies/libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb && sudo dpkg -i libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb && rm libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb"
target "apt-get clean"
@@ -191,10 +221,8 @@
copyfile root.root 644 etc/udev/rules.d/99-mali.rules
target "apt-get update"
-target "apt-get -y install -t bullseye-backports systemd"
target "systemctl enable systemd-networkd"
-target "systemctl enable systemd-resolved"
target "systemctl enable grow-rootfs"
target "systemctl enable frc971"
target "systemctl enable frc971chrt"
@@ -206,14 +234,14 @@
git clone --separate-git-dir=/home/pi/.dotfiles https://github.com/AustinSchuh/.dotfiles.git tmpdotfiles && \
rsync --recursive --verbose --exclude .git tmpdotfiles/ /home/pi/ && \
rm -r tmpdotfiles && \
- git --git-dir=/home/pi/.dotfiles/ --work-tree=/home/pi/ config --local status.showUntrackedFiles no && \
- vim -c \":qa!\""
+ git --git-dir=/home/pi/.dotfiles/ --work-tree=/home/pi/ config --local status.showUntrackedFiles no"
+ pi_target "vim -c \":qa!\""
target "cd /root/ && \
git clone --separate-git-dir=/root/.dotfiles https://github.com/AustinSchuh/.dotfiles.git tmpdotfiles && \
rsync --recursive --verbose --exclude .git tmpdotfiles/ /root/ && rm -r tmpdotfiles && \
- git --git-dir=/root/.dotfiles/ --work-tree=/root/ config --local status.showUntrackedFiles no && \
- vim -c \":qa!\""
+ git --git-dir=/root/.dotfiles/ --work-tree=/root/ config --local status.showUntrackedFiles no"
+ target "vim -c \":qa!\""
fi
target "apt-get clean"
@@ -221,7 +249,7 @@
sudo chroot ${PARTITION} qemu-aarch64-static /bin/bash
# TODO(austin): This appears to not be working... pi_target doesn't apper to be happy
-sudo chroot --userspec=pi.pi ${PARTITION} qemu-aarch64-static /bin/bash
+sudo chroot --userspec=pi:pi --groups=pi ${PARTITION} qemu-aarch64-static /bin/bash
# TODO(austin): everything else we were doing to the pi's.
sudo rm ${PARTITION}/usr/bin/qemu-aarch64-static
diff --git a/frc971/rockpi/contents/etc/systemd/system/frc971.service b/frc971/rockpi/contents/etc/systemd/system/frc971.service
index be6b37f..d109b0f 100644
--- a/frc971/rockpi/contents/etc/systemd/system/frc971.service
+++ b/frc971/rockpi/contents/etc/systemd/system/frc971.service
@@ -4,8 +4,8 @@
After=network-online.target
[Service]
-User=pi
-Group=pi
+User=root
+Group=root
Type=simple
WorkingDirectory=/home/pi/bin
ExecStart=/home/pi/bin/starter.sh
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 195634e..642ca0f 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -1,4 +1,5 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_py_library")
+load("//aos:config.bzl", "aos_config")
load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
flatbuffer_cc_library(
@@ -46,6 +47,21 @@
visibility = ["//visibility:public"],
)
+py_library(
+ name = "create_calib_file",
+ srcs = [
+ "create_calib_file.py",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":calibration_fbs_python",
+ "@bazel_tools//tools/python/runfiles",
+ "@pip//glog",
+ "@pip//opencv_python",
+ ],
+)
+
cc_library(
name = "v4l2_reader",
srcs = [
@@ -61,6 +77,7 @@
"//aos/events:epoll",
"//aos/events:event_loop",
"//aos/scoped:scoped_fd",
+ "//aos/util:threaded_consumer",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
@@ -87,6 +104,7 @@
"//y2020/vision/sift:sift_fbs",
"//y2020/vision/sift:sift_training_fbs",
"//y2020/vision/tools/python_code:sift_training_data",
+ "@com_github_foxglove_schemas//:schemas",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/strings:str_format",
"@com_google_absl//absl/types:span",
@@ -106,6 +124,7 @@
visibility = ["//visibility:public"],
deps = [
":charuco_lib",
+ ":foxglove_image_converter",
"//aos:init",
"//aos/events/logging:log_reader",
"//frc971/analysis:in_process_plotter",
@@ -114,6 +133,8 @@
"//frc971/wpilib:imu_batch_fbs",
"//frc971/wpilib:imu_fbs",
"//third_party:opencv",
+ "@com_github_foxglove_schemas//:CompressedImage_schema",
+ "@com_github_foxglove_schemas//:ImageAnnotations_schema",
"@com_google_absl//absl/strings:str_format",
"@com_google_ceres_solver//:ceres",
"@org_tuxfamily_eigen//:eigen",
@@ -140,7 +161,7 @@
":target_map_fbs",
"//aos/events:simulated_event_loop",
"//frc971/control_loops:control_loop",
- "//frc971/vision/ceres:pose_graph_2d_lib",
+ "//frc971/vision/ceres:pose_graph_3d_lib",
"//third_party:opencv",
"@com_google_ceres_solver//:ceres",
"@org_tuxfamily_eigen//:eigen",
@@ -152,12 +173,15 @@
srcs = [
"target_mapper_test.cc",
],
+ data = [":target_map.json"],
target_compatible_with = ["@platforms//os:linux"],
deps = [
":target_mapper",
"//aos/events:simulated_event_loop",
"//aos/testing:googletest",
+ "//aos/testing:path",
"//aos/testing:random_seed",
+ "//aos/util:math",
],
)
@@ -233,3 +257,49 @@
"@com_google_absl//absl/strings",
],
)
+
+cc_library(
+ name = "foxglove_image_converter",
+ srcs = ["foxglove_image_converter.cc"],
+ hdrs = ["foxglove_image_converter.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":charuco_lib",
+ ":vision_fbs",
+ "//aos/events:event_loop",
+ "//third_party:opencv",
+ "@com_github_foxglove_schemas//:schemas",
+ ],
+)
+
+aos_config(
+ name = "converter_config",
+ testonly = True,
+ src = "converter_test_config.json",
+ flatbuffers = [
+ "//frc971/vision:vision_fbs",
+ "//aos/events:event_loop_fbs",
+ "//aos/logging:log_message_fbs",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "@com_github_foxglove_schemas//:schemas",
+ ],
+)
+
+cc_test(
+ name = "foxglove_image_converter_test",
+ srcs = ["foxglove_image_converter_test.cc"],
+ data = [
+ ":converter_config",
+ "@april_tag_test_image",
+ ],
+ deps = [
+ ":foxglove_image_converter",
+ "//aos:configuration",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ "//aos/testing:path",
+ "//aos/testing:tmpdir",
+ ],
+)
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index eee22f5..ea9af28 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -11,6 +11,8 @@
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/charuco_lib.h"
#include "frc971/wpilib/imu_batch_generated.h"
+#include "external/com_github_foxglove_schemas/ImageAnnotations_schema.h"
+#include "external/com_github_foxglove_schemas/CompressedImage_schema.h"
DEFINE_bool(display_undistorted, false,
"If true, display the undistorted image.");
@@ -111,6 +113,33 @@
}
}
+CalibrationFoxgloveVisualizer::CalibrationFoxgloveVisualizer(
+ aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ image_converter_(event_loop_, "/camera", "/visualization",
+ ImageCompression::kJpeg),
+ annotations_sender_(
+ event_loop_->MakeSender<foxglove::ImageAnnotations>("/visualization")) {}
+
+aos::FlatbufferDetachedBuffer<aos::Configuration>
+CalibrationFoxgloveVisualizer::AddVisualizationChannels(
+ const aos::Configuration *config, const aos::Node *node) {
+ constexpr std::string_view channel_name = "/visualization";
+ aos::ChannelT channel_overrides;
+ channel_overrides.max_size = 10000000;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> result =
+ aos::configuration::AddChannelToConfiguration(
+ config, channel_name,
+ aos::FlatbufferSpan<reflection::Schema>(
+ foxglove::ImageAnnotationsSchema()),
+ node, channel_overrides);
+ return aos::configuration::AddChannelToConfiguration(
+ &result.message(), channel_name,
+ aos::FlatbufferSpan<reflection::Schema>(
+ foxglove::CompressedImageSchema()),
+ node, channel_overrides);
+}
+
Calibration::Calibration(aos::SimulatedEventLoopFactory *event_loop_factory,
aos::EventLoop *image_event_loop,
aos::EventLoop *imu_event_loop, std::string_view pi,
@@ -140,7 +169,9 @@
[this](cv::Mat rgb_image, const monotonic_clock::time_point eof) {
charuco_extractor_.HandleImage(rgb_image, eof);
}),
- data_(data) {
+ data_(data),
+ visualizer_event_loop_(image_factory_->MakeEventLoop("visualization")),
+ visualizer_(visualizer_event_loop_.get()) {
imu_factory_->OnShutdown([]() { cv::destroyAllWindows(); });
// Check for IMUValuesBatch topic on both /localizer and /drivetrain channels,
@@ -173,9 +204,10 @@
void Calibration::HandleCharuco(
cv::Mat rgb_image, const monotonic_clock::time_point eof,
std::vector<cv::Vec4i> /*charuco_ids*/,
- std::vector<std::vector<cv::Point2f>> /*charuco_corners*/, bool valid,
+ std::vector<std::vector<cv::Point2f>> charuco_corners, bool valid,
std::vector<Eigen::Vector3d> rvecs_eigen,
std::vector<Eigen::Vector3d> tvecs_eigen) {
+ visualizer_.HandleCharuco(eof, charuco_corners);
if (valid) {
CHECK(rvecs_eigen.size() > 0) << "Require at least one target detected";
// We only use one (the first) target detected for calibration
@@ -237,6 +269,7 @@
last_value_.accelerometer_y,
last_value_.accelerometer_z);
+ // TODO: ToDistributedClock may be too noisy.
data_->AddImu(imu_factory_->ToDistributedClock(monotonic_clock::time_point(
chrono::nanoseconds(imu->monotonic_timestamp_ns()))),
gyro, accel * kG);
diff --git a/frc971/vision/calibration_accumulator.h b/frc971/vision/calibration_accumulator.h
index d9f6065..5c435ad 100644
--- a/frc971/vision/calibration_accumulator.h
+++ b/frc971/vision/calibration_accumulator.h
@@ -8,6 +8,7 @@
#include "aos/time/time.h"
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/foxglove_image_converter.h"
#include "frc971/wpilib/imu_batch_generated.h"
namespace frc971 {
@@ -76,6 +77,28 @@
turret_points_;
};
+class CalibrationFoxgloveVisualizer {
+ public:
+ CalibrationFoxgloveVisualizer(aos::EventLoop *event_loop);
+
+ static aos::FlatbufferDetachedBuffer<aos::Configuration>
+ AddVisualizationChannels(const aos::Configuration *config,
+ const aos::Node *node);
+
+ void HandleCharuco(const aos::monotonic_clock::time_point eof,
+ std::vector<std::vector<cv::Point2f>> charuco_corners) {
+ auto builder = annotations_sender_.MakeBuilder();
+ builder.CheckOk(
+ builder.Send(BuildAnnotations(eof, charuco_corners, builder.fbb())));
+ }
+
+ private:
+ aos::EventLoop *event_loop_;
+ FoxgloveImageConverter image_converter_;
+
+ aos::Sender<foxglove::ImageAnnotations> annotations_sender_;
+};
+
// Class to register image and IMU callbacks in AOS and route them to the
// corresponding CalibrationData class.
class Calibration {
@@ -110,6 +133,9 @@
CalibrationData *data_;
+ std::unique_ptr<aos::EventLoop> visualizer_event_loop_;
+ CalibrationFoxgloveVisualizer visualizer_;
+
frc971::IMUValuesT last_value_;
};
diff --git a/frc971/vision/ceres/BUILD b/frc971/vision/ceres/BUILD
index 5622b55..f2cb96b 100644
--- a/frc971/vision/ceres/BUILD
+++ b/frc971/vision/ceres/BUILD
@@ -1,10 +1,8 @@
# Copy of ceres example code from their repo
cc_library(
- name = "pose_graph_2d_lib",
+ name = "pose_graph_3d_lib",
hdrs = [
- "angle_local_parameterization.h",
- "normalize_angle.h",
- "pose_graph_2d_error_term.h",
+ "pose_graph_3d_error_term.h",
"read_g2o.h",
"types.h",
],
diff --git a/frc971/vision/ceres/angle_local_parameterization.h b/frc971/vision/ceres/angle_local_parameterization.h
deleted file mode 100644
index a81637c..0000000
--- a/frc971/vision/ceres/angle_local_parameterization.h
+++ /dev/null
@@ -1,64 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
-// http://ceres-solver.org/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: vitus@google.com (Michael Vitus)
-
-#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
-#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
-
-#include "ceres/local_parameterization.h"
-#include "normalize_angle.h"
-
-namespace ceres {
-namespace examples {
-
-// Defines a local parameterization for updating the angle to be constrained in
-// [-pi to pi).
-class AngleLocalParameterization {
- public:
- template <typename T>
- bool operator()(const T* theta_radians,
- const T* delta_theta_radians,
- T* theta_radians_plus_delta) const {
- *theta_radians_plus_delta =
- NormalizeAngle(*theta_radians + *delta_theta_radians);
-
- return true;
- }
-
- static ceres::LocalParameterization* Create() {
- return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
- 1,
- 1>);
- }
-};
-
-} // namespace examples
-} // namespace ceres
-
-#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
diff --git a/frc971/vision/ceres/normalize_angle.h b/frc971/vision/ceres/normalize_angle.h
deleted file mode 100644
index c215671..0000000
--- a/frc971/vision/ceres/normalize_angle.h
+++ /dev/null
@@ -1,53 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
-// http://ceres-solver.org/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: vitus@google.com (Michael Vitus)
-
-#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
-#define CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
-
-#include <cmath>
-
-#include "ceres/ceres.h"
-
-namespace ceres {
-namespace examples {
-
-// Normalizes the angle in radians between [-pi and pi).
-template <typename T>
-inline T NormalizeAngle(const T& angle_radians) {
- // Use ceres::floor because it is specialized for double and Jet types.
- T two_pi(2.0 * M_PI);
- return angle_radians -
- two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
-}
-
-} // namespace examples
-} // namespace ceres
-
-#endif // CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
diff --git a/frc971/vision/ceres/pose_graph_2d_error_term.h b/frc971/vision/ceres/pose_graph_2d_error_term.h
deleted file mode 100644
index 2df31f6..0000000
--- a/frc971/vision/ceres/pose_graph_2d_error_term.h
+++ /dev/null
@@ -1,119 +0,0 @@
-// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2016 Google Inc. All rights reserved.
-// http://ceres-solver.org/
-//
-// Redistribution and use in source and binary forms, with or without
-// modification, are permitted provided that the following conditions are met:
-//
-// * Redistributions of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-// * Redistributions in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-// * Neither the name of Google Inc. nor the names of its contributors may be
-// used to endorse or promote products derived from this software without
-// specific prior written permission.
-//
-// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// POSSIBILITY OF SUCH DAMAGE.
-//
-// Author: vitus@google.com (Michael Vitus)
-//
-// Cost function for a 2D pose graph formulation.
-
-#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
-#define CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
-
-#include "Eigen/Core"
-
-namespace ceres {
-namespace examples {
-
-template <typename T>
-Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) {
- const T cos_yaw = ceres::cos(yaw_radians);
- const T sin_yaw = ceres::sin(yaw_radians);
-
- Eigen::Matrix<T, 2, 2> rotation;
- rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
- return rotation;
-}
-
-// Computes the error term for two poses that have a relative pose measurement
-// between them. Let the hat variables be the measurement.
-//
-// residual = information^{1/2} * [ r_a^T * (p_b - p_a) - \hat{p_ab} ]
-// [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
-//
-// where r_a is the rotation matrix that rotates a vector represented in frame A
-// into the global frame, and Normalize(*) ensures the angles are in the range
-// [-pi, pi).
-class PoseGraph2dErrorTerm {
- public:
- PoseGraph2dErrorTerm(double x_ab,
- double y_ab,
- double yaw_ab_radians,
- const Eigen::Matrix3d& sqrt_information)
- : p_ab_(x_ab, y_ab),
- yaw_ab_radians_(yaw_ab_radians),
- sqrt_information_(sqrt_information) {}
-
- template <typename T>
- bool operator()(const T* const x_a,
- const T* const y_a,
- const T* const yaw_a,
- const T* const x_b,
- const T* const y_b,
- const T* const yaw_b,
- T* residuals_ptr) const {
- const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
- const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
-
- Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals_map(residuals_ptr);
-
- residuals_map.template head<2>() =
- RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) - p_ab_.cast<T>();
- residuals_map(2) = ceres::examples::NormalizeAngle(
- (*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
-
- // Scale the residuals by the square root information matrix to account for
- // the measurement uncertainty.
- residuals_map = sqrt_information_.template cast<T>() * residuals_map;
-
- return true;
- }
-
- static ceres::CostFunction* Create(double x_ab,
- double y_ab,
- double yaw_ab_radians,
- const Eigen::Matrix3d& sqrt_information) {
- return (new ceres::
- AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
- new PoseGraph2dErrorTerm(
- x_ab, y_ab, yaw_ab_radians, sqrt_information)));
- }
-
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- private:
- // The position of B relative to A in the A frame.
- const Eigen::Vector2d p_ab_;
- // The orientation of frame B relative to frame A.
- const double yaw_ab_radians_;
- // The inverse square root of the measurement covariance matrix.
- const Eigen::Matrix3d sqrt_information_;
-};
-
-} // namespace examples
-} // namespace ceres
-
-#endif // CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
diff --git a/frc971/vision/ceres/pose_graph_3d_error_term.h b/frc971/vision/ceres/pose_graph_3d_error_term.h
new file mode 100644
index 0000000..1f3e8de
--- /dev/null
+++ b/frc971/vision/ceres/pose_graph_3d_error_term.h
@@ -0,0 +1,132 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2016 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Michael Vitus)
+
+#ifndef EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
+#define EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
+
+#include "Eigen/Core"
+#include "ceres/autodiff_cost_function.h"
+#include "types.h"
+
+namespace ceres {
+namespace examples {
+
+// Computes the error term for two poses that have a relative pose measurement
+// between them. Let the hat variables be the measurement. We have two poses x_a
+// and x_b. Through sensor measurements we can measure the transformation of
+// frame B w.r.t frame A denoted as t_ab_hat. We can compute an error metric
+// between the current estimate of the poses and the measurement.
+//
+// In this formulation, we have chosen to represent the rigid transformation as
+// a Hamiltonian quaternion, q, and position, p. The quaternion ordering is
+// [x, y, z, w].
+
+// The estimated measurement is:
+// t_ab = [ p_ab ] = [ R(q_a)^T * (p_b - p_a) ]
+// [ q_ab ] [ q_a^{-1] * q_b ]
+//
+// where ^{-1} denotes the inverse and R(q) is the rotation matrix for the
+// quaternion. Now we can compute an error metric between the estimated and
+// measurement transformation. For the orientation error, we will use the
+// standard multiplicative error resulting in:
+//
+// error = [ p_ab - \hat{p}_ab ]
+// [ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]
+//
+// where Vec(*) returns the vector (imaginary) part of the quaternion. Since
+// the measurement has an uncertainty associated with how accurate it is, we
+// will weight the errors by the square root of the measurement information
+// matrix:
+//
+// residuals = I^{1/2) * error
+// where I is the information matrix which is the inverse of the covariance.
+class PoseGraph3dErrorTerm {
+ public:
+ PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
+ const Eigen::Matrix<double, 6, 6>& sqrt_information)
+ : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
+
+ template <typename T>
+ bool operator()(const T* const p_a_ptr,
+ const T* const q_a_ptr,
+ const T* const p_b_ptr,
+ const T* const q_b_ptr,
+ T* residuals_ptr) const {
+ Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);
+ Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);
+
+ Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_b(p_b_ptr);
+ Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr);
+
+ // Compute the relative transformation between the two frames.
+ Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
+ Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
+
+ // Represent the displacement between the two frames in the A frame.
+ Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
+
+ // Compute the error between the two orientation estimates.
+ Eigen::Quaternion<T> delta_q =
+ t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();
+
+ // Compute the residuals.
+ // [ position ] [ delta_p ]
+ // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
+ Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr);
+ residuals.template block<3, 1>(0, 0) =
+ p_ab_estimated - t_ab_measured_.p.template cast<T>();
+ residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
+
+ // Scale the residuals by the measurement uncertainty.
+ residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
+
+ return true;
+ }
+
+ static ceres::CostFunction* Create(
+ const Pose3d& t_ab_measured,
+ const Eigen::Matrix<double, 6, 6>& sqrt_information) {
+ return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
+ new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
+ }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+ // The measurement for the position of B relative to A in the A frame.
+ const Pose3d t_ab_measured_;
+ // The square root of the measurement information matrix.
+ const Eigen::Matrix<double, 6, 6> sqrt_information_;
+};
+
+} // namespace examples
+} // namespace ceres
+
+#endif // EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
diff --git a/frc971/vision/ceres/types.h b/frc971/vision/ceres/types.h
index 3c13824..d3f19ed 100644
--- a/frc971/vision/ceres/types.h
+++ b/frc971/vision/ceres/types.h
@@ -27,75 +27,86 @@
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: vitus@google.com (Michael Vitus)
-//
-// Defines the types used in the 2D pose graph SLAM formulation. Each vertex of
-// the graph has a unique integer ID with a position and orientation. There are
-// delta transformation constraints between two vertices.
-#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
-#define CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
+#ifndef EXAMPLES_CERES_TYPES_H_
+#define EXAMPLES_CERES_TYPES_H_
-#include <fstream>
+#include <istream>
+#include <map>
+#include <string>
+#include <vector>
#include "Eigen/Core"
-#include "normalize_angle.h"
+#include "Eigen/Geometry"
namespace ceres {
namespace examples {
-// The state for each vertex in the pose graph.
-struct Pose2d {
- double x;
- double y;
- double yaw_radians;
+struct Pose3d {
+ Eigen::Vector3d p;
+ Eigen::Quaterniond q;
// The name of the data type in the g2o file format.
- static std::string name() { return "VERTEX_SE2"; }
+ static std::string name() { return "VERTEX_SE3:QUAT"; }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-inline std::istream& operator>>(std::istream& input, Pose2d& pose) {
- input >> pose.x >> pose.y >> pose.yaw_radians;
- // Normalize the angle between -pi to pi.
- pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
+inline std::istream& operator>>(std::istream& input, Pose3d& pose) {
+ input >> pose.p.x() >> pose.p.y() >> pose.p.z() >> pose.q.x() >> pose.q.y() >>
+ pose.q.z() >> pose.q.w();
+ // Normalize the quaternion to account for precision loss due to
+ // serialization.
+ pose.q.normalize();
return input;
}
+typedef std::map<int,
+ Pose3d,
+ std::less<int>,
+ Eigen::aligned_allocator<std::pair<const int, Pose3d>>>
+ MapOfPoses;
+
// The constraint between two vertices in the pose graph. The constraint is the
// transformation from vertex id_begin to vertex id_end.
-struct Constraint2d {
+struct Constraint3d {
int id_begin;
int id_end;
- double x;
- double y;
- double yaw_radians;
+ // The transformation that represents the pose of the end frame E w.r.t. the
+ // begin frame B. In other words, it transforms a vector in the E frame to
+ // the B frame.
+ Pose3d t_be;
// The inverse of the covariance matrix for the measurement. The order of the
- // entries are x, y, and yaw.
- Eigen::Matrix3d information;
+ // entries are x, y, z, delta orientation.
+ Eigen::Matrix<double, 6, 6> information;
// The name of the data type in the g2o file format.
- static std::string name() { return "EDGE_SE2"; }
+ static std::string name() { return "EDGE_SE3:QUAT"; }
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
-inline std::istream& operator>>(std::istream& input, Constraint2d& constraint) {
- input >> constraint.id_begin >> constraint.id_end >> constraint.x >>
- constraint.y >> constraint.yaw_radians >> constraint.information(0, 0) >>
- constraint.information(0, 1) >> constraint.information(0, 2) >>
- constraint.information(1, 1) >> constraint.information(1, 2) >>
- constraint.information(2, 2);
+inline std::istream& operator>>(std::istream& input, Constraint3d& constraint) {
+ Pose3d& t_be = constraint.t_be;
+ input >> constraint.id_begin >> constraint.id_end >> t_be;
- // Set the lower triangular part of the information matrix.
- constraint.information(1, 0) = constraint.information(0, 1);
- constraint.information(2, 0) = constraint.information(0, 2);
- constraint.information(2, 1) = constraint.information(1, 2);
-
- // Normalize the angle between -pi to pi.
- constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians);
+ for (int i = 0; i < 6 && input.good(); ++i) {
+ for (int j = i; j < 6 && input.good(); ++j) {
+ input >> constraint.information(i, j);
+ if (i != j) {
+ constraint.information(j, i) = constraint.information(i, j);
+ }
+ }
+ }
return input;
}
+typedef std::vector<Constraint3d, Eigen::aligned_allocator<Constraint3d>>
+ VectorOfConstraints;
+
} // namespace examples
} // namespace ceres
-#endif // CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
+#endif // EXAMPLES_CERES_TYPES_H_
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index c4d98d9..f12a6a5 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -27,7 +27,6 @@
"The mininum number of aruco targets in charuco board required to match.");
DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
-DEFINE_uint32(age, 5, "Age to start dropping frames at.");
DEFINE_uint32(disable_delay, 100, "Time after an issue to disable tracing at.");
DECLARE_bool(enable_ftrace);
@@ -94,8 +93,8 @@
ImageCallback::ImageCallback(
aos::EventLoop *event_loop, std::string_view channel,
- std::function<void(cv::Mat, monotonic_clock::time_point)> &&handle_image_fn)
-
+ std::function<void(cv::Mat, monotonic_clock::time_point)> &&handle_image_fn,
+ monotonic_clock::duration max_age)
: event_loop_(event_loop),
server_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
@@ -106,7 +105,8 @@
->source_node()
->string_view())),
handle_image_(std::move(handle_image_fn)),
- timer_fn_(event_loop->AddTimer([this]() { DisableTracing(); })) {
+ timer_fn_(event_loop->AddTimer([this]() { DisableTracing(); })),
+ max_age_(max_age) {
event_loop_->MakeWatcher(channel, [this](const CameraImage &image) {
const monotonic_clock::time_point eof_source_node =
monotonic_clock::time_point(
@@ -145,7 +145,7 @@
const monotonic_clock::duration age = event_loop_->monotonic_now() - eof;
const double age_double =
std::chrono::duration_cast<std::chrono::duration<double>>(age).count();
- if (age > std::chrono::milliseconds(FLAGS_age)) {
+ if (age > max_age_) {
if (FLAGS_enable_ftrace) {
ftrace_.FormatMessage("Too late receiving image, age: %f\n",
age_double);
@@ -230,12 +230,6 @@
marker_length_ = 0.15;
square_length_ = 0.1651;
dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250);
- } else if (target_type_ == TargetType::kAprilTag) {
- // Tag will be 6 in (15.24 cm) according to
- // https://www.firstinspires.org/robotics/frc/blog/2022-2023-approved-devices-rules-preview-and-vision-target-update
- square_length_ = 0.1524;
- dictionary_ =
- cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_16h5);
} else {
// Bail out if it's not a supported target
LOG(FATAL) << "Target type undefined: "
@@ -435,9 +429,8 @@
<< ", not enough marker IDs for charuco board, got "
<< marker_ids.size() << ", needed " << FLAGS_min_charucos;
}
- } else if (target_type_ == TargetType::kAprilTag ||
- target_type_ == TargetType::kAruco) {
- // estimate pose for april tags doesn't return valid, so marking true
+ } else if (target_type_ == TargetType::kAruco) {
+ // estimate pose for arucos doesn't return valid, so marking true
valid = true;
std::vector<cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(marker_corners, square_length_,
@@ -487,5 +480,42 @@
rvecs_eigen, tvecs_eigen);
}
+flatbuffers::Offset<foxglove::ImageAnnotations> BuildAnnotations(
+ const aos::monotonic_clock::time_point monotonic_now,
+ const std::vector<std::vector<cv::Point2f>> &corners,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ std::vector<flatbuffers::Offset<foxglove::PointsAnnotation>> rectangles;
+ const struct timespec now_t = aos::time::to_timespec(monotonic_now);
+ foxglove::Time time{static_cast<uint32_t>(now_t.tv_sec),
+ static_cast<uint32_t>(now_t.tv_nsec)};
+ const flatbuffers::Offset<foxglove::Color> color_offset =
+ foxglove::CreateColor(*fbb, 0.0, 1.0, 0.0, 1.0);
+ for (const std::vector<cv::Point2f> &rectangle : corners) {
+ std::vector<flatbuffers::Offset<foxglove::Point2>> points_offsets;
+ for (const cv::Point2f &point : rectangle) {
+ points_offsets.push_back(foxglove::CreatePoint2(*fbb, point.x, point.y));
+ }
+ const flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<foxglove::Point2>>>
+ points_offset = fbb->CreateVector(points_offsets);
+ std::vector<flatbuffers::Offset<foxglove::Color>> color_offsets(
+ points_offsets.size(), color_offset);
+ auto colors_offset = fbb->CreateVector(color_offsets);
+ foxglove::PointsAnnotation::Builder points_builder(*fbb);
+ points_builder.add_timestamp(&time);
+ points_builder.add_type(foxglove::PointsAnnotationType::POINTS);
+ points_builder.add_points(points_offset);
+ points_builder.add_outline_color(color_offset);
+ points_builder.add_outline_colors(colors_offset);
+ points_builder.add_thickness(2.0);
+ rectangles.push_back(points_builder.Finish());
+ }
+
+ const auto rectangles_offset = fbb->CreateVector(rectangles);
+ foxglove::ImageAnnotations::Builder annotation_builder(*fbb);
+ annotation_builder.add_points(rectangles_offset);
+ return annotation_builder.Finish();
+}
+
} // namespace vision
} // namespace frc971
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index c7269f9..984cef6 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -13,6 +13,7 @@
#include "aos/network/message_bridge_server_generated.h"
#include "y2020/vision/sift/sift_generated.h"
#include "y2020/vision/sift/sift_training_generated.h"
+#include "external/com_github_foxglove_schemas/ImageAnnotations_generated.h"
DECLARE_bool(visualize);
@@ -54,13 +55,15 @@
BGR = 1,
GRAYSCALE = 2,
};
- ImageCallback(aos::EventLoop *event_loop, std::string_view channel,
- std::function<void(cv::Mat, aos::monotonic_clock::time_point)>
- &&handle_image_fn);
- void set_format(Format format) {
- format_ = format;
- }
+ // `max_age` is the age to start dropping frames at
+ ImageCallback(
+ aos::EventLoop *event_loop, std::string_view channel,
+ std::function<void(cv::Mat, aos::monotonic_clock::time_point)>
+ &&handle_image_fn,
+ aos::monotonic_clock::duration max_age = std::chrono::milliseconds(100));
+
+ void set_format(Format format) { format_ = format; }
private:
void DisableTracing();
@@ -76,14 +79,15 @@
aos::Ftrace ftrace_;
Format format_ = Format::BGR;
+
+ aos::monotonic_clock::duration max_age_;
};
// Types of targets that a CharucoExtractor can detect in images
enum class TargetType : uint8_t {
- kAprilTag = 0,
- kAruco = 1,
- kCharuco = 2,
- kCharucoDiamond = 3
+ kAruco = 0,
+ kCharuco = 1,
+ kCharucoDiamond = 2
};
// Class which calls a callback each time an image arrives with the information
@@ -175,6 +179,13 @@
handle_charuco_;
};
+// Puts the provided charuco corners into a foxglove ImageAnnotation type for
+// visualization purposes.
+flatbuffers::Offset<foxglove::ImageAnnotations> BuildAnnotations(
+ const aos::monotonic_clock::time_point monotonic_now,
+ const std::vector<std::vector<cv::Point2f>> &corners,
+ flatbuffers::FlatBufferBuilder *fbb);
+
} // namespace vision
} // namespace frc971
diff --git a/frc971/vision/converter_test_config.json b/frc971/vision/converter_test_config.json
new file mode 100644
index 0000000..5d74dd1
--- /dev/null
+++ b/frc971/vision/converter_test_config.json
@@ -0,0 +1,46 @@
+{
+ "channels" : [
+ {
+ "name": "/aos",
+ "type": "aos.timing.Report",
+ "source_node": "test"
+ },
+ {
+ "name": "/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "test"
+ },
+ {
+ "name": "/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "test"
+ },
+ {
+ "name": "/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "test"
+ },
+ {
+ "name": "/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "test"
+ },
+ {
+ "name": "/camera",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "test",
+ "max_size": 10000000
+ },
+ {
+ "name": "/visualize",
+ "type": "foxglove.CompressedImage",
+ "source_node": "test",
+ "max_size": 10000000
+ }
+ ],
+ "nodes": [
+ {
+ "name": "test"
+ }
+ ]
+}
diff --git a/frc971/vision/create_calib_file.py b/frc971/vision/create_calib_file.py
new file mode 100644
index 0000000..1ef881d
--- /dev/null
+++ b/frc971/vision/create_calib_file.py
@@ -0,0 +1,175 @@
+import copy
+import cv2
+import flatbuffers
+import glog
+import json
+import numpy as np
+import os
+import sys
+
+import frc971.vision.calibration.CalibrationData as CalibrationData
+import frc971.vision.calibration.CameraCalibration as CameraCalibration
+import frc971.vision.calibration.TransformationMatrix as TransformationMatrix
+
+import glog
+
+
+def bazel_name_fix(filename, year):
+ """Quick fix to naming games that happen with bazel"""
+ ret_name = filename
+ try:
+ from bazel_tools.tools.python.runfiles import runfiles
+ r = runfiles.Create()
+ ret_name = r.Rlocation('org_frc971/y%s/vision/%s' % (year, filename))
+ except:
+ print("Failed bazel_name_fix")
+ pass
+
+ return ret_name
+
+
+def list_to_transformation_matrix(values, fbb):
+ """Puts a list of values into an FBB TransformationMatrix."""
+
+ TransformationMatrix.TransformationMatrixStartDataVector(fbb, len(values))
+ for n in reversed(values):
+ fbb.PrependFloat32(n)
+ list_offset = fbb.EndVector()
+
+ TransformationMatrix.TransformationMatrixStart(fbb)
+ TransformationMatrix.TransformationMatrixAddData(fbb, list_offset)
+ return TransformationMatrix.TransformationMatrixEnd(fbb)
+
+
+def load_camera_definitions(year):
+ """
+ CAMERA DEFINITIONS
+ We only load in cameras that have a calibration file
+ These are stored in y<year>/vision/calib_files
+
+ Or better yet, use //y2020/vision:calibration to calibrate the camera
+ using a Charuco target board
+ """
+
+ dir_name = bazel_name_fix("calib_files", year)
+ if dir_name is not None:
+ glog.debug("Searching for calibration files in " + dir_name)
+ else:
+ glog.fatal("Failed to find calib_files directory")
+
+ camera_calib_list = []
+ for filename in sorted(os.listdir(dir_name)):
+ glog.debug("Inspecting %s", filename)
+ if ("cam-calib-int" in filename
+ or 'calibration' in filename) and filename.endswith(".json"):
+ # Extract intrinsics from file
+ calib_file = open(dir_name + "/" + filename, 'r')
+ calib_dict = json.loads(calib_file.read())
+ camera_calib_list.append(calib_dict)
+ return camera_calib_list
+
+
+def generate_header(year):
+ """
+ Generates a header file with a CalibrationData() function
+ returning the data as a binary flatbuffer.
+ Expects command line argument: output header path
+ Parameter year is a string, ex. "2023"
+ """
+ camera_calib_list = load_camera_definitions(year)
+
+ output_path = sys.argv[1]
+ glog.debug("Writing file to %s", output_path)
+
+ fbb = flatbuffers.Builder(0)
+
+ images_vector = []
+
+ # Create camera calibration data
+ camera_calibration_vector = []
+ for camera_calib in camera_calib_list:
+ fixed_extrinsics_list = camera_calib["fixed_extrinsics"]
+ fixed_extrinsics_vector = list_to_transformation_matrix(
+ fixed_extrinsics_list, fbb)
+
+ turret_extrinsics_vector = None
+ if "turret_extrinsics" in camera_calib:
+ turret_extrinsics_list = camera_calib["turret_extrinsics"]
+ turret_extrinsics_vector = list_to_transformation_matrix(
+ turret_extrinsics_list, fbb)
+
+ camera_int_list = camera_calib["intrinsics"]
+ CameraCalibration.CameraCalibrationStartIntrinsicsVector(
+ fbb, len(camera_int_list))
+ for n in reversed(camera_int_list):
+ fbb.PrependFloat32(n)
+ intrinsics_vector = fbb.EndVector()
+
+ dist_coeffs_list = camera_calib["dist_coeffs"]
+ CameraCalibration.CameraCalibrationStartDistCoeffsVector(
+ fbb, len(dist_coeffs_list))
+ for n in reversed(dist_coeffs_list):
+ fbb.PrependFloat32(n)
+ dist_coeffs_vector = fbb.EndVector()
+
+ node_name_offset = fbb.CreateString(camera_calib["node_name"])
+ CameraCalibration.CameraCalibrationStart(fbb)
+ CameraCalibration.CameraCalibrationAddNodeName(fbb, node_name_offset)
+ CameraCalibration.CameraCalibrationAddTeamNumber(
+ fbb, camera_calib["team_number"])
+ CameraCalibration.CameraCalibrationAddIntrinsics(
+ fbb, intrinsics_vector)
+ CameraCalibration.CameraCalibrationAddDistCoeffs(
+ fbb, dist_coeffs_vector)
+ CameraCalibration.CameraCalibrationAddFixedExtrinsics(
+ fbb, fixed_extrinsics_vector)
+ if turret_extrinsics_vector is not None:
+ CameraCalibration.CameraCalibrationAddTurretExtrinsics(
+ fbb, turret_extrinsics_vector)
+ camera_calibration_vector.append(
+ CameraCalibration.CameraCalibrationEnd(fbb))
+
+ CalibrationData.CalibrationDataStartCameraCalibrationsVector(
+ fbb, len(camera_calibration_vector))
+ for camera_calibration in reversed(camera_calibration_vector):
+ fbb.PrependUOffsetTRelative(camera_calibration)
+ camera_calibration_vector_table = fbb.EndVector()
+
+ # Fill out TrainingData
+ CalibrationData.CalibrationDataStart(fbb)
+ CalibrationData.CalibrationDataAddCameraCalibrations(
+ fbb, camera_calibration_vector_table)
+ fbb.Finish(CalibrationData.CalibrationDataEnd(fbb))
+
+ bfbs = fbb.Output()
+
+ # "year" will get replaced by the variable
+ output_prefix = [
+ '#ifndef Y{year}_VISION_CALIBRATION_DATA_H_',
+ '#define Y{year}_VISION_CALIBRATION_DATA_H_',
+ '#include <stdint.h>',
+ '#include "absl/types/span.h"',
+ 'namespace y{year} {{',
+ 'namespace vision {{',
+ 'inline absl::Span<const uint8_t> CalibrationData() {{',
+ ]
+ output_suffix = [
+ ' return absl::Span<const uint8_t>(reinterpret_cast<const uint8_t *>(kData), sizeof(kData));',
+ '}}',
+ '}} // namespace vision',
+ '}} // namespace y{year}',
+ '#endif // Y{year}_VISION_CALIBRATION_DATA_H_',
+ ]
+
+ # Write out the header file
+ with open(output_path, 'wb') as output:
+ for line in output_prefix:
+ output.write(line.format(year=year).encode("utf-8"))
+ output.write(b'\n')
+ output.write(b'alignas(64) static constexpr char kData[] = "')
+ for byte in fbb.Output():
+ output.write(b'\\x' + (b'%x' % byte).zfill(2))
+ output.write(b'";\n')
+ for line in output_suffix:
+ output.write(line.format(year=year).encode("utf-8"))
+ output.write(b'\n')
diff --git a/frc971/vision/foxglove_image_converter.cc b/frc971/vision/foxglove_image_converter.cc
new file mode 100644
index 0000000..abde78b
--- /dev/null
+++ b/frc971/vision/foxglove_image_converter.cc
@@ -0,0 +1,52 @@
+#include "frc971/vision/foxglove_image_converter.h"
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+
+namespace frc971::vision {
+std::string_view ExtensionForCompression(ImageCompression compression) {
+ switch (compression) {
+ case ImageCompression::kJpeg:
+ return "jpeg";
+ case ImageCompression::kPng:
+ return "png";
+ }
+}
+
+flatbuffers::Offset<foxglove::CompressedImage> CompressImage(
+ const cv::Mat image, const aos::monotonic_clock::time_point eof,
+ flatbuffers::FlatBufferBuilder *fbb, ImageCompression compression) {
+ std::string_view format = ExtensionForCompression(compression);
+ // imencode doesn't let us pass in anything other than an std::vector, and
+ // performance isn't yet a big enough issue to try to avoid the copy.
+ std::vector<uint8_t> buffer;
+ CHECK(cv::imencode(absl::StrCat(".", format), image, buffer));
+ const flatbuffers::Offset<flatbuffers::Vector<uint8_t>> data_offset =
+ fbb->CreateVector(buffer);
+ const struct timespec timestamp_t = aos::time::to_timespec(eof);
+ const foxglove::Time time{static_cast<uint32_t>(timestamp_t.tv_sec),
+ static_cast<uint32_t>(timestamp_t.tv_nsec)};
+ const flatbuffers::Offset<flatbuffers::String> format_offset =
+ fbb->CreateString(format);
+ foxglove::CompressedImage::Builder builder(*fbb);
+ builder.add_timestamp(&time);
+ builder.add_data(data_offset);
+ builder.add_format(format_offset);
+ return builder.Finish();
+}
+
+FoxgloveImageConverter::FoxgloveImageConverter(aos::EventLoop *event_loop,
+ std::string_view input_channel,
+ std::string_view output_channel,
+ ImageCompression compression)
+ : event_loop_(event_loop),
+ image_callback_(
+ event_loop_, input_channel,
+ [this, compression](const cv::Mat image,
+ const aos::monotonic_clock::time_point eof) {
+ auto builder = sender_.MakeBuilder();
+ builder.CheckOk(builder.Send(
+ CompressImage(image, eof, builder.fbb(), compression)));
+ }),
+ sender_(
+ event_loop_->MakeSender<foxglove::CompressedImage>(output_channel)) {}
+} // namespace frc971::vision
diff --git a/frc971/vision/foxglove_image_converter.h b/frc971/vision/foxglove_image_converter.h
new file mode 100644
index 0000000..872ac14
--- /dev/null
+++ b/frc971/vision/foxglove_image_converter.h
@@ -0,0 +1,40 @@
+#ifndef FRC971_VISION_FOXGLOVE_IMAGE_CONVERTER_H_
+#define FRC971_VISION_FOXGLOVE_IMAGE_CONVERTER_H_
+#include "aos/events/event_loop.h"
+#include "external/com_github_foxglove_schemas/CompressedImage_generated.h"
+#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/vision_generated.h"
+
+namespace frc971::vision {
+// Empirically, from 2022 logs:
+// PNG is an ~2x space savings relative to raw images.
+// JPEG is an ~10x space savings relative to PNG.
+// Both perform significantly better than attempting to perform in-browser
+// conversion with a user-script in Foxglove Studio.
+enum class ImageCompression { kJpeg, kPng };
+
+std::string_view ExtensionForCompression(ImageCompression compression);
+
+flatbuffers::Offset<foxglove::CompressedImage> CompressImage(
+ const cv::Mat image, const aos::monotonic_clock::time_point eof,
+ flatbuffers::FlatBufferBuilder *fbb, ImageCompression compression);
+
+// This class provides a simple converter that will take an AOS CameraImage
+// channel and output
+class FoxgloveImageConverter {
+ public:
+ // Watches for frc971.vision.CameraImage messages on the input_channel and
+ // sends foxglove.CompressedImage messages on the output_channel, using the
+ // specified image compression algorithm.
+ FoxgloveImageConverter(aos::EventLoop *event_loop,
+ std::string_view input_channel,
+ std::string_view output_channel,
+ ImageCompression compression);
+
+ private:
+ aos::EventLoop *event_loop_;
+ ImageCallback image_callback_;
+ aos::Sender<foxglove::CompressedImage> sender_;
+};
+} // namespace frc971::vision
+#endif // FRC971_VISION_FOXGLOVE_IMAGE_CONVERTER_H_
diff --git a/frc971/vision/foxglove_image_converter_test.cc b/frc971/vision/foxglove_image_converter_test.cc
new file mode 100644
index 0000000..65b3b6b
--- /dev/null
+++ b/frc971/vision/foxglove_image_converter_test.cc
@@ -0,0 +1,68 @@
+#include "frc971/vision/foxglove_image_converter.h"
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/testing/path.h"
+#include "aos/testing/tmpdir.h"
+#include "gtest/gtest.h"
+
+namespace frc971::vision {
+std::ostream &operator<<(std::ostream &os, ImageCompression compression) {
+ os << ExtensionForCompression(compression);
+ return os;
+}
+namespace testing {
+class ImageConverterTest : public ::testing::TestWithParam<ImageCompression> {
+ protected:
+ ImageConverterTest()
+ : config_(aos::configuration::ReadConfig(
+ aos::testing::ArtifactPath("frc971/vision/converter_config.json"))),
+ factory_(&config_.message()),
+ camera_image_(
+ aos::FileToFlatbuffer<CameraImage>(aos::testing::ArtifactPath(
+ "external/april_tag_test_image/test.bfbs"))),
+ node_(aos::configuration::GetNode(&config_.message(), "test")),
+ test_event_loop_(factory_.MakeEventLoop("test", node_)),
+ image_sender_(test_event_loop_->MakeSender<CameraImage>("/camera")),
+ converter_event_loop_(factory_.MakeEventLoop("converter", node_)),
+ converter_(converter_event_loop_.get(), "/camera", "/visualize",
+ GetParam()),
+ output_path_(absl::StrCat(aos::testing::TestTmpDir(), "/test.",
+ ExtensionForCompression(GetParam()))) {
+ test_event_loop_->OnRun(
+ [this]() { image_sender_.CheckOk(image_sender_.Send(camera_image_)); });
+ test_event_loop_->MakeWatcher(
+ "/visualize", [this](const foxglove::CompressedImage &image) {
+ ASSERT_TRUE(image.has_data());
+ std::string expected_contents =
+ aos::util::ReadFileToStringOrDie(aos::testing::ArtifactPath(
+ absl::StrCat("external/april_tag_test_image/expected.",
+ ExtensionForCompression(GetParam()))));
+ std::string_view data(
+ reinterpret_cast<const char *>(image.data()->data()),
+ image.data()->size());
+ EXPECT_EQ(expected_contents, data);
+ aos::util::WriteStringToFileOrDie(output_path_, data);
+ factory_.Exit();
+ });
+ }
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::SimulatedEventLoopFactory factory_;
+ aos::FlatbufferVector<CameraImage> camera_image_;
+ const aos::Node *const node_;
+ std::unique_ptr<aos::EventLoop> test_event_loop_;
+ aos::Sender<CameraImage> image_sender_;
+ std::unique_ptr<aos::EventLoop> converter_event_loop_;
+ FoxgloveImageConverter converter_;
+ std::string output_path_;
+};
+
+TEST_P(ImageConverterTest, ImageToFoxglove) { factory_.Run(); }
+
+INSTANTIATE_TEST_SUITE_P(CompressionOptions, ImageConverterTest,
+ ::testing::Values(ImageCompression::kJpeg,
+ ImageCompression::kPng));
+
+} // namespace testing
+} // namespace frc971::vision
diff --git a/frc971/vision/target_map.fbs b/frc971/vision/target_map.fbs
index 50a9d7d..cee2b07 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -1,27 +1,44 @@
namespace frc971.vision;
+table Position {
+ x:double (id: 0);
+ y:double (id: 1);
+ z:double (id: 2);
+}
+
+table Quaternion {
+ w:double (id: 0);
+ x:double (id: 1);
+ y:double (id: 2);
+ z:double (id: 3);
+}
+
// Represents 3d pose of an april tag on the field.
table TargetPoseFbs {
// AprilTag ID of this target
id:uint64 (id: 0);
- // Pose of target relative to field origin.
- // NOTE: As of now, we only solve for the 2d pose (x, y, yaw)
- // and all other values will be 0.
- x:double (id: 1);
- y:double (id: 2);
- z:double (id: 3);
-
- roll:double (id: 4);
- pitch:double (id: 5);
- yaw:double (id: 6);
+ // Pose of target relative to either the field origin or robot.
+ // To get the pose of the target, do:
+ // Translation3d(position.x(), position.y(), position.z()) *
+ // Quaterniond(orientation.w(), orientation.x(), orientation.y(), orientation.z())
+ position:Position (id: 1);
+ orientation:Quaternion (id: 2);
}
// Map of all target poses on a field.
-// This would be solved for by TargetMapper
+// There are two possible uses for this:
+// 1. Static april tag poses on the field solved for by TargetMapper.
+// 2. List of detected april poses relative to the robot.
table TargetMap {
target_poses:[TargetPoseFbs] (id: 0);
- // Unique name of the field
+ // Unique name of the field (for use case 1.)
field_name:string (id: 1);
-}
\ No newline at end of file
+
+ // End-of-frame timestamp for the frame with tag detections.
+ // (for use case 2.).
+ monotonic_timestamp_ns:int64 (id: 2);
+}
+
+root_type TargetMap;
diff --git a/frc971/vision/target_map.json b/frc971/vision/target_map.json
index 73156f1..3f6eb54 100644
--- a/frc971/vision/target_map.json
+++ b/frc971/vision/target_map.json
@@ -1,22 +1,124 @@
{
- "target_poses": [
- {
- "id": 584,
- "x": 0.0,
- "y": 0.0,
- "yaw": 0.0
- },
- {
- "id": 585,
- "x": 0.0,
- "y": 0.0,
- "yaw": 0.0
- },
- {
- "id": 586,
- "x": 0.0,
- "y": 0.0,
- "yaw": 0.0
- }
- ]
-}
\ No newline at end of file
+ "target_poses": [
+ {
+ "id": 1,
+ "position": {
+ "x": 7.244,
+ "y": -2.938,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
+ },
+ {
+ "id": 2,
+ "position": {
+ "x": 7.244,
+ "y": -1.262,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
+ },
+ {
+ "id": 3,
+ "position": {
+ "x": 7.244,
+ "y": 0.414,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
+ },
+ {
+ "id": 4,
+ "position": {
+ "x": 7.909,
+ "y": 2.740,
+ "z": 0.695
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
+ },
+ {
+ "id": 5,
+ "position": {
+ "x": -7.908,
+ "y": 2.740,
+ "z": 0.695
+ },
+ "orientation": {
+ /* yaw of 0 */
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
+ },
+ {
+ "id": 6,
+ "position": {
+ "x": -7.243,
+ "y": 0.414,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
+ },
+ {
+ "id": 7,
+ "position": {
+ "x": -7.243,
+ "y": -1.262,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
+ },
+ {
+ "id": 8,
+ "position": {
+ "x": -7.243,
+ "y": -2.938,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
+ }
+ ]
+}
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 9222039..b3049e4 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -1,9 +1,8 @@
#include "frc971/vision/target_mapper.h"
+#include "absl/strings/str_format.h"
#include "frc971/control_loops/control_loop.h"
-#include "frc971/vision/ceres/angle_local_parameterization.h"
-#include "frc971/vision/ceres/normalize_angle.h"
-#include "frc971/vision/ceres/pose_graph_2d_error_term.h"
+#include "frc971/vision/ceres/pose_graph_3d_error_term.h"
#include "frc971/vision/geometry.h"
DEFINE_uint64(max_num_iterations, 100,
@@ -11,161 +10,89 @@
namespace frc971::vision {
-Eigen::Affine3d PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d pose2d) {
+Eigen::Affine3d PoseUtils::Pose3dToAffine3d(
+ const ceres::examples::Pose3d &pose3d) {
Eigen::Affine3d H_world_pose =
- Eigen::Translation3d(pose2d.x, pose2d.y, 0.0) *
- Eigen::AngleAxisd(pose2d.yaw_radians, Eigen::Vector3d::UnitZ());
+ Eigen::Translation3d(pose3d.p(0), pose3d.p(1), pose3d.p(2)) * pose3d.q;
return H_world_pose;
}
-ceres::examples::Pose2d PoseUtils::Affine3dToPose2d(Eigen::Affine3d H) {
- Eigen::Vector3d T = H.translation();
- double heading = std::atan2(H.rotation()(1, 0), H.rotation()(0, 0));
- return ceres::examples::Pose2d{T[0], T[1],
- ceres::examples::NormalizeAngle(heading)};
+ceres::examples::Pose3d PoseUtils::Affine3dToPose3d(const Eigen::Affine3d &H) {
+ return ceres::examples::Pose3d{.p = H.translation(),
+ .q = Eigen::Quaterniond(H.rotation())};
}
-ceres::examples::Pose2d PoseUtils::ComputeRelativePose(
- ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2) {
- Eigen::Affine3d H_world_1 = Pose2dToAffine3d(pose_1);
- Eigen::Affine3d H_world_2 = Pose2dToAffine3d(pose_2);
+ceres::examples::Pose3d PoseUtils::ComputeRelativePose(
+ const ceres::examples::Pose3d &pose_1,
+ const ceres::examples::Pose3d &pose_2) {
+ Eigen::Affine3d H_world_1 = Pose3dToAffine3d(pose_1);
+ Eigen::Affine3d H_world_2 = Pose3dToAffine3d(pose_2);
// Get the location of 2 in the 1 frame
Eigen::Affine3d H_1_2 = H_world_1.inverse() * H_world_2;
- return Affine3dToPose2d(H_1_2);
+ return Affine3dToPose3d(H_1_2);
}
-ceres::examples::Pose2d PoseUtils::ComputeOffsetPose(
- ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2_relative) {
- auto H_world_1 = Pose2dToAffine3d(pose_1);
- auto H_1_2 = Pose2dToAffine3d(pose_2_relative);
+ceres::examples::Pose3d PoseUtils::ComputeOffsetPose(
+ const ceres::examples::Pose3d &pose_1,
+ const ceres::examples::Pose3d &pose_2_relative) {
+ auto H_world_1 = Pose3dToAffine3d(pose_1);
+ auto H_1_2 = Pose3dToAffine3d(pose_2_relative);
auto H_world_2 = H_world_1 * H_1_2;
- return Affine3dToPose2d(H_world_2);
+ return Affine3dToPose3d(H_world_2);
}
-namespace {
-double ExponentiatedSinTerm(double theta) {
- return (theta == 0.0 ? 1.0 : std::sin(theta) / theta);
+Eigen::Quaterniond PoseUtils::EulerAnglesToQuaternion(
+ const Eigen::Vector3d &rpy) {
+ Eigen::AngleAxisd roll_angle(rpy.x(), Eigen::Vector3d::UnitX());
+ Eigen::AngleAxisd pitch_angle(rpy.y(), Eigen::Vector3d::UnitY());
+ Eigen::AngleAxisd yaw_angle(rpy.z(), Eigen::Vector3d::UnitZ());
+
+ return yaw_angle * pitch_angle * roll_angle;
}
-double ExponentiatedCosTerm(double theta) {
- return (theta == 0.0 ? 0.0 : (1 - std::cos(theta)) / theta);
-}
-} // namespace
-
-ceres::examples::Pose2d DataAdapter::InterpolatePose(
- const TimestampedPose &pose_start, const TimestampedPose &pose_end,
- aos::distributed_clock::time_point time) {
- auto delta_pose =
- PoseUtils::ComputeRelativePose(pose_start.pose, pose_end.pose);
- // Time from start of period, on the scale 0-1 where 1 is the end.
- double interpolation_scalar =
- static_cast<double>((time - pose_start.time).count()) /
- static_cast<double>((pose_end.time - pose_start.time).count());
-
- double theta = delta_pose.yaw_radians;
- // Take the log of the transformation matrix:
- // https://mathoverflow.net/questions/118533/how-to-compute-se2-group-exponential-and-logarithm
- StdFormLine dx_line = {.a = ExponentiatedSinTerm(theta),
- .b = -ExponentiatedCosTerm(theta),
- .c = delta_pose.x};
- StdFormLine dy_line = {.a = ExponentiatedCosTerm(theta),
- .b = ExponentiatedSinTerm(theta),
- .c = delta_pose.y};
-
- std::optional<cv::Point2d> solution = dx_line.Intersection(dy_line);
- CHECK(solution.has_value());
-
- // Re-exponentiate with the new values scaled by the interpolation scalar to
- // get an interpolated tranformation matrix
- double a = solution->x * interpolation_scalar;
- double b = solution->y * interpolation_scalar;
- double alpha = theta * interpolation_scalar;
-
- ceres::examples::Pose2d interpolated_pose = {
- .x = a * ExponentiatedSinTerm(theta) - b * ExponentiatedCosTerm(theta),
- .y = a * ExponentiatedCosTerm(theta) + b * ExponentiatedSinTerm(theta),
- .yaw_radians = alpha};
-
- return PoseUtils::ComputeOffsetPose(pose_start.pose, interpolated_pose);
-} // namespace frc971::vision
-
-std::pair<std::vector<ceres::examples::Constraint2d>,
- std::vector<ceres::examples::Pose2d>>
-DataAdapter::MatchTargetDetections(
- const std::vector<TimestampedPose> ×tamped_robot_poses,
- const std::vector<TimestampedDetection> ×tamped_target_detections) {
- // Interpolate robot poses
- std::map<aos::distributed_clock::time_point, ceres::examples::Pose2d>
- interpolated_poses;
-
- CHECK_GT(timestamped_robot_poses.size(), 1ul)
- << "Need more than 1 robot pose";
- auto robot_pose_it = timestamped_robot_poses.begin();
- for (const auto ×tamped_detection : timestamped_target_detections) {
- aos::distributed_clock::time_point target_time = timestamped_detection.time;
-
- // Skip this target detection if we have no robot poses before it
- if (robot_pose_it->time > target_time) {
- continue;
- }
-
- // Find the robot point right before this localization
- while (robot_pose_it->time > target_time ||
- (robot_pose_it + 1)->time <= target_time) {
- robot_pose_it++;
- CHECK(robot_pose_it < timestamped_robot_poses.end() - 1)
- << "Need a robot pose before and after every target detection";
- }
-
- auto start = robot_pose_it;
- auto end = robot_pose_it + 1;
- interpolated_poses.emplace(target_time,
- InterpolatePose(*start, *end, target_time));
- }
-
- // In the case that all target detections were before the first robot
- // detection, we would have no interpolated poses at this point
- CHECK_GT(interpolated_poses.size(), 0ul)
- << "Need a robot pose before and after every target detection";
-
- // Match consecutive detections
- std::vector<ceres::examples::Constraint2d> target_constraints;
- std::vector<ceres::examples::Pose2d> robot_delta_poses;
-
- auto last_detection = timestamped_target_detections[0];
- auto last_robot_pose =
- interpolated_poses[timestamped_target_detections[0].time];
-
- for (auto it = timestamped_target_detections.begin() + 1;
- it < timestamped_target_detections.end(); it++) {
- // Skip two consecutive detections of the same target, because the solver
- // doesn't allow this
- if (it->id == last_detection.id) {
- continue;
- }
-
- auto robot_pose = interpolated_poses[it->time];
- auto robot_delta_pose =
- PoseUtils::ComputeRelativePose(last_robot_pose, robot_pose);
- auto confidence = ComputeConfidence(last_detection.time, it->time,
- last_detection.distance_from_camera,
- it->distance_from_camera);
-
- target_constraints.emplace_back(ComputeTargetConstraint(
- last_detection, PoseUtils::Pose2dToAffine3d(robot_delta_pose), *it,
- confidence));
- robot_delta_poses.emplace_back(robot_delta_pose);
-
- last_detection = *it;
- last_robot_pose = robot_pose;
- }
-
- return {target_constraints, robot_delta_poses};
+Eigen::Vector3d PoseUtils::QuaternionToEulerAngles(
+ const Eigen::Quaterniond &q) {
+ return RotationMatrixToEulerAngles(q.toRotationMatrix());
}
-std::vector<ceres::examples::Constraint2d> DataAdapter::MatchTargetDetections(
+Eigen::Vector3d PoseUtils::RotationMatrixToEulerAngles(
+ const Eigen::Matrix3d &R) {
+ double roll = aos::math::NormalizeAngle(std::atan2(R(2, 1), R(2, 2)));
+ double pitch = aos::math::NormalizeAngle(-std::asin(R(2, 0)));
+ double yaw = aos::math::NormalizeAngle(std::atan2(R(1, 0), R(0, 0)));
+
+ return Eigen::Vector3d(roll, pitch, yaw);
+}
+
+flatbuffers::Offset<TargetPoseFbs> PoseUtils::TargetPoseToFbs(
+ const TargetMapper::TargetPose &target_pose,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const auto position_offset =
+ CreatePosition(*fbb, target_pose.pose.p(0), target_pose.pose.p(1),
+ target_pose.pose.p(2));
+ const auto orientation_offset =
+ CreateQuaternion(*fbb, target_pose.pose.q.w(), target_pose.pose.q.x(),
+ target_pose.pose.q.y(), target_pose.pose.q.z());
+ return CreateTargetPoseFbs(*fbb, target_pose.id, position_offset,
+ orientation_offset);
+}
+
+TargetMapper::TargetPose PoseUtils::TargetPoseFromFbs(
+ const TargetPoseFbs &target_pose_fbs) {
+ return {.id = static_cast<TargetMapper::TargetId>(target_pose_fbs.id()),
+ .pose = ceres::examples::Pose3d{
+ Eigen::Vector3d(target_pose_fbs.position()->x(),
+ target_pose_fbs.position()->y(),
+ target_pose_fbs.position()->z()),
+ Eigen::Quaterniond(target_pose_fbs.orientation()->w(),
+ target_pose_fbs.orientation()->x(),
+ target_pose_fbs.orientation()->y(),
+ target_pose_fbs.orientation()->z())}};
+}
+
+ceres::examples::VectorOfConstraints DataAdapter::MatchTargetDetections(
const std::vector<DataAdapter::TimestampedDetection>
×tamped_target_detections,
aos::distributed_clock::duration max_dt) {
@@ -173,7 +100,7 @@
<< "Must have at least 2 detections";
// Match consecutive detections
- std::vector<ceres::examples::Constraint2d> target_constraints;
+ ceres::examples::VectorOfConstraints target_constraints;
for (auto it = timestamped_target_detections.begin() + 1;
it < timestamped_target_detections.end(); it++) {
auto last_detection = *(it - 1);
@@ -200,23 +127,30 @@
return target_constraints;
}
-Eigen::Matrix3d DataAdapter::ComputeConfidence(
+TargetMapper::ConfidenceMatrix DataAdapter::ComputeConfidence(
aos::distributed_clock::time_point start,
aos::distributed_clock::time_point end, double distance_from_camera_start,
double distance_from_camera_end) {
constexpr size_t kX = 0;
constexpr size_t kY = 1;
- constexpr size_t kTheta = 2;
+ constexpr size_t kZ = 2;
+ constexpr size_t kOrientation1 = 3;
+ constexpr size_t kOrientation2 = 4;
+ constexpr size_t kOrientation3 = 5;
// Uncertainty matrix between start and end
- Eigen::Matrix3d P = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix P = TargetMapper::ConfidenceMatrix::Zero();
{
// Noise for odometry-based robot position measurements
- Eigen::Matrix3d Q_odometry = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix Q_odometry =
+ TargetMapper::ConfidenceMatrix::Zero();
Q_odometry(kX, kX) = std::pow(0.045, 2);
Q_odometry(kY, kY) = std::pow(0.045, 2);
- Q_odometry(kTheta, kTheta) = std::pow(0.01, 2);
+ Q_odometry(kZ, kZ) = std::pow(0.045, 2);
+ Q_odometry(kOrientation1, kOrientation1) = std::pow(0.01, 2);
+ Q_odometry(kOrientation2, kOrientation2) = std::pow(0.01, 2);
+ Q_odometry(kOrientation3, kOrientation3) = std::pow(0.01, 2);
// Add uncertainty for robot position measurements from start to end
int iterations = (end - start) / frc971::controls::kLoopFrequency;
@@ -227,10 +161,14 @@
// Noise for vision-based target localizations. Multiplying this matrix by
// the distance from camera to target squared results in the uncertainty in
// that measurement
- Eigen::Matrix3d Q_vision = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix Q_vision =
+ TargetMapper::ConfidenceMatrix::Zero();
Q_vision(kX, kX) = std::pow(0.045, 2);
Q_vision(kY, kY) = std::pow(0.045, 2);
- Q_vision(kTheta, kTheta) = std::pow(0.02, 2);
+ Q_vision(kZ, kZ) = std::pow(0.045, 2);
+ Q_vision(kOrientation1, kOrientation1) = std::pow(0.02, 2);
+ Q_vision(kOrientation2, kOrientation2) = std::pow(0.02, 2);
+ Q_vision(kOrientation3, kOrientation3) = std::pow(0.02, 2);
// Add uncertainty for the 2 vision measurements (1 at start and 1 at end)
P += Q_vision * std::pow(distance_from_camera_start, 2);
@@ -240,48 +178,39 @@
return P.inverse();
}
-ceres::examples::Constraint2d DataAdapter::ComputeTargetConstraint(
+ceres::examples::Constraint3d DataAdapter::ComputeTargetConstraint(
const TimestampedDetection &target_detection_start,
- const Eigen::Affine3d &H_robotstart_robotend,
const TimestampedDetection &target_detection_end,
- const Eigen::Matrix3d &confidence) {
+ const TargetMapper::ConfidenceMatrix &confidence) {
// Compute the relative pose (constraint) between the two targets
Eigen::Affine3d H_targetstart_targetend =
- target_detection_start.H_robot_target.inverse() * H_robotstart_robotend *
+ target_detection_start.H_robot_target.inverse() *
target_detection_end.H_robot_target;
- ceres::examples::Pose2d target_constraint =
- PoseUtils::Affine3dToPose2d(H_targetstart_targetend);
+ ceres::examples::Pose3d target_constraint =
+ PoseUtils::Affine3dToPose3d(H_targetstart_targetend);
- return ceres::examples::Constraint2d{
- target_detection_start.id, target_detection_end.id,
- target_constraint.x, target_constraint.y,
- target_constraint.yaw_radians, confidence};
-}
-
-ceres::examples::Constraint2d DataAdapter::ComputeTargetConstraint(
- const TimestampedDetection &target_detection_start,
- const TimestampedDetection &target_detection_end,
- const Eigen::Matrix3d &confidence) {
- return ComputeTargetConstraint(target_detection_start,
- Eigen::Affine3d(Eigen::Matrix4d::Identity()),
- target_detection_end, confidence);
+ return ceres::examples::Constraint3d{
+ target_detection_start.id,
+ target_detection_end.id,
+ {target_constraint.p, target_constraint.q},
+ confidence};
}
TargetMapper::TargetMapper(
std::string_view target_poses_path,
- std::vector<ceres::examples::Constraint2d> target_constraints)
+ const ceres::examples::VectorOfConstraints &target_constraints)
: target_constraints_(target_constraints) {
aos::FlatbufferDetachedBuffer<TargetMap> target_map =
aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
- target_poses_[target_pose_fbs->id()] = ceres::examples::Pose2d{
- target_pose_fbs->x(), target_pose_fbs->y(), target_pose_fbs->yaw()};
+ target_poses_[target_pose_fbs->id()] =
+ PoseUtils::TargetPoseFromFbs(*target_pose_fbs).pose;
}
}
TargetMapper::TargetMapper(
- std::map<TargetId, ceres::examples::Pose2d> target_poses,
- std::vector<ceres::examples::Constraint2d> target_constraints)
+ const ceres::examples::MapOfPoses &target_poses,
+ const ceres::examples::VectorOfConstraints &target_constraints)
: target_poses_(target_poses), target_constraints_(target_constraints) {}
std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById(
@@ -295,71 +224,70 @@
return std::nullopt;
}
-// Taken from ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
+// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
+// Constructs the nonlinear least squares optimization problem from the pose
+// graph constraints.
void TargetMapper::BuildOptimizationProblem(
- std::map<int, ceres::examples::Pose2d> *poses,
- const std::vector<ceres::examples::Constraint2d> &constraints,
- ceres::Problem *problem) {
- CHECK_NOTNULL(poses);
- CHECK_NOTNULL(problem);
+ const ceres::examples::VectorOfConstraints &constraints,
+ ceres::examples::MapOfPoses *poses, ceres::Problem *problem) {
+ CHECK(poses != nullptr);
+ CHECK(problem != nullptr);
if (constraints.empty()) {
- LOG(WARNING) << "No constraints, no problem to optimize.";
+ LOG(INFO) << "No constraints, no problem to optimize.";
return;
}
ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0);
- ceres::LocalParameterization *angle_local_parameterization =
- ceres::examples::AngleLocalParameterization::Create();
+ ceres::LocalParameterization *quaternion_local_parameterization =
+ new ceres::EigenQuaternionParameterization;
- for (std::vector<ceres::examples::Constraint2d>::const_iterator
- constraints_iter = constraints.begin();
+ for (ceres::examples::VectorOfConstraints::const_iterator constraints_iter =
+ constraints.begin();
constraints_iter != constraints.end(); ++constraints_iter) {
- const ceres::examples::Constraint2d &constraint = *constraints_iter;
+ const ceres::examples::Constraint3d &constraint = *constraints_iter;
- std::map<int, ceres::examples::Pose2d>::iterator pose_begin_iter =
+ ceres::examples::MapOfPoses::iterator pose_begin_iter =
poses->find(constraint.id_begin);
CHECK(pose_begin_iter != poses->end())
<< "Pose with ID: " << constraint.id_begin << " not found.";
- std::map<int, ceres::examples::Pose2d>::iterator pose_end_iter =
+ ceres::examples::MapOfPoses::iterator pose_end_iter =
poses->find(constraint.id_end);
CHECK(pose_end_iter != poses->end())
<< "Pose with ID: " << constraint.id_end << " not found.";
- const Eigen::Matrix3d sqrt_information =
+ const Eigen::Matrix<double, 6, 6> sqrt_information =
constraint.information.llt().matrixL();
// Ceres will take ownership of the pointer.
ceres::CostFunction *cost_function =
- ceres::examples::PoseGraph2dErrorTerm::Create(
- constraint.x, constraint.y, constraint.yaw_radians,
- sqrt_information);
- problem->AddResidualBlock(
- cost_function, loss_function, &pose_begin_iter->second.x,
- &pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians,
- &pose_end_iter->second.x, &pose_end_iter->second.y,
- &pose_end_iter->second.yaw_radians);
+ ceres::examples::PoseGraph3dErrorTerm::Create(constraint.t_be,
+ sqrt_information);
- problem->SetParameterization(&pose_begin_iter->second.yaw_radians,
- angle_local_parameterization);
- problem->SetParameterization(&pose_end_iter->second.yaw_radians,
- angle_local_parameterization);
+ problem->AddResidualBlock(cost_function, loss_function,
+ pose_begin_iter->second.p.data(),
+ pose_begin_iter->second.q.coeffs().data(),
+ pose_end_iter->second.p.data(),
+ pose_end_iter->second.q.coeffs().data());
+
+ problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
+ quaternion_local_parameterization);
+ problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
+ quaternion_local_parameterization);
}
- // The pose graph optimization problem has three DOFs that are not fully
- // constrained. This is typically referred to as gauge freedom. You can apply
- // a rigid body transformation to all the nodes and the optimization problem
- // will still have the exact same cost. The Levenberg-Marquardt algorithm has
- // internal damping which mitigates this issue, but it is better to properly
- // constrain the gauge freedom. This can be done by setting one of the poses
- // as constant so the optimizer cannot change it.
- std::map<int, ceres::examples::Pose2d>::iterator pose_start_iter =
- poses->begin();
+ // The pose graph optimization problem has six DOFs that are not fully
+ // constrained. This is typically referred to as gauge freedom. You can
+ // apply a rigid body transformation to all the nodes and the optimization
+ // problem will still have the exact same cost. The Levenberg-Marquardt
+ // algorithm has internal damping which mitigates this issue, but it is
+ // better to properly constrain the gauge freedom. This can be done by
+ // setting one of the poses as constant so the optimizer cannot change it.
+ ceres::examples::MapOfPoses::iterator pose_start_iter = poses->begin();
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
- problem->SetParameterBlockConstant(&pose_start_iter->second.x);
- problem->SetParameterBlockConstant(&pose_start_iter->second.y);
- problem->SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
+ problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
+ problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
}
-// Taken from ceres/examples/slam/pose_graph_2d/pose_graph_2d.cc
+// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
bool TargetMapper::SolveOptimizationProblem(ceres::Problem *problem) {
CHECK_NOTNULL(problem);
@@ -378,13 +306,11 @@
void TargetMapper::Solve(std::string_view field_name,
std::optional<std::string_view> output_dir) {
ceres::Problem problem;
- BuildOptimizationProblem(&target_poses_, target_constraints_, &problem);
+ BuildOptimizationProblem(target_constraints_, &target_poses_, &problem);
CHECK(SolveOptimizationProblem(&problem))
<< "The solve was not successful, exiting.";
- // TODO(milind): add origin to first target offset to all poses
-
auto map_json = MapToJson(field_name);
VLOG(1) << "Solved target poses: " << map_json;
@@ -402,13 +328,8 @@
// Convert poses to flatbuffers
std::vector<flatbuffers::Offset<TargetPoseFbs>> target_poses_fbs;
for (const auto &[id, pose] : target_poses_) {
- TargetPoseFbs::Builder target_pose_builder(fbb);
- target_pose_builder.add_id(id);
- target_pose_builder.add_x(pose.x);
- target_pose_builder.add_y(pose.y);
- target_pose_builder.add_yaw(pose.yaw_radians);
-
- target_poses_fbs.emplace_back(target_pose_builder.Finish());
+ target_poses_fbs.emplace_back(
+ PoseUtils::TargetPoseToFbs(TargetPose{.id = id, .pose = pose}, &fbb));
}
const auto field_name_offset = fbb.CreateString(field_name);
@@ -420,4 +341,21 @@
{.multi_line = true});
}
+std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose) {
+ auto rpy = PoseUtils::QuaternionToEulerAngles(pose.q);
+ os << absl::StrFormat(
+ "{x: %.3f, y: %.3f, z: %.3f, roll: %.3f, pitch: "
+ "%.3f, yaw: %.3f}",
+ pose.p(0), pose.p(1), pose.p(2), rpy(0), rpy(1), rpy(2));
+ return os;
+}
+
+std::ostream &operator<<(std::ostream &os,
+ ceres::examples::Constraint3d constraint) {
+ os << absl::StrFormat("{id_begin: %d, id_end: %d, pose: ",
+ constraint.id_begin, constraint.id_end)
+ << constraint.t_be << "}";
+ return os;
+}
+
} // namespace frc971::vision
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index edf2260..7701fd1 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -4,6 +4,7 @@
#include <unordered_map>
#include "aos/events/simulated_event_loop.h"
+#include "ceres/ceres.h"
#include "frc971/vision/ceres/types.h"
#include "frc971/vision/target_map_generated.h"
@@ -16,12 +17,11 @@
class TargetMapper {
public:
using TargetId = int;
+ using ConfidenceMatrix = Eigen::Matrix<double, 6, 6>;
struct TargetPose {
TargetId id;
- // TOOD(milind): switch everything to 3d once we're more confident in 2d
- // solving
- ceres::examples::Pose2d pose;
+ ceres::examples::Pose3d pose;
};
// target_poses_path is the path to a TargetMap json with initial guesses for
@@ -29,11 +29,11 @@
// target_constraints are the deltas between consecutive target detections,
// and are usually prepared by the DataAdapter class below.
TargetMapper(std::string_view target_poses_path,
- std::vector<ceres::examples::Constraint2d> target_constraints);
+ const ceres::examples::VectorOfConstraints &target_constraints);
// Alternate constructor for tests.
// Takes in the actual intial guesses instead of a file containing them
- TargetMapper(std::map<TargetId, ceres::examples::Pose2d> target_poses,
- std::vector<ceres::examples::Constraint2d> target_constraints);
+ TargetMapper(const ceres::examples::MapOfPoses &target_poses,
+ const ceres::examples::VectorOfConstraints &target_constraints);
// Solves for the target map. If output_dir is set, the map will be saved to
// output_dir/field_name.json
@@ -46,56 +46,63 @@
static std::optional<TargetPose> GetTargetPoseById(
std::vector<TargetPose> target_poses, TargetId target_id);
- std::map<TargetId, ceres::examples::Pose2d> target_poses() {
- return target_poses_;
- }
+ ceres::examples::MapOfPoses target_poses() { return target_poses_; }
private:
// Constructs the nonlinear least squares optimization problem from the
// pose graph constraints.
void BuildOptimizationProblem(
- std::map<TargetId, ceres::examples::Pose2d> *target_poses,
- const std::vector<ceres::examples::Constraint2d> &constraints,
- ceres::Problem *problem);
+ const ceres::examples::VectorOfConstraints &constraints,
+ ceres::examples::MapOfPoses *poses, ceres::Problem *problem);
// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem *problem);
- std::map<TargetId, ceres::examples::Pose2d> target_poses_;
- std::vector<ceres::examples::Constraint2d> target_constraints_;
+ ceres::examples::MapOfPoses target_poses_;
+ ceres::examples::VectorOfConstraints target_constraints_;
};
-// Utility functions for dealing with ceres::examples::Pose2d structs
+// Utility functions for dealing with ceres::examples::Pose3d structs
class PoseUtils {
public:
- // Embeds a 2d pose into a 3d affine transformation to be used in 3d
- // computation
- static Eigen::Affine3d Pose2dToAffine3d(ceres::examples::Pose2d pose2d);
- // Assumes only x and y translation, and only z rotation (yaw)
- static ceres::examples::Pose2d Affine3dToPose2d(Eigen::Affine3d H);
+ // Embeds a 3d pose into an affine transformation
+ static Eigen::Affine3d Pose3dToAffine3d(
+ const ceres::examples::Pose3d &pose3d);
+ // Inverse of above function
+ static ceres::examples::Pose3d Affine3dToPose3d(const Eigen::Affine3d &H);
// Computes pose_2 relative to pose_1. This is equivalent to (pose_1^-1 *
// pose_2)
- static ceres::examples::Pose2d ComputeRelativePose(
- ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2);
+ static ceres::examples::Pose3d ComputeRelativePose(
+ const ceres::examples::Pose3d &pose_1,
+ const ceres::examples::Pose3d &pose_2);
// Computes pose_2 given a pose_1 and pose_2 relative to pose_1. This is
// equivalent to (pose_1 * pose_2_relative)
- static ceres::examples::Pose2d ComputeOffsetPose(
- ceres::examples::Pose2d pose_1, ceres::examples::Pose2d pose_2_relative);
+ static ceres::examples::Pose3d ComputeOffsetPose(
+ const ceres::examples::Pose3d &pose_1,
+ const ceres::examples::Pose3d &pose_2_relative);
+
+ // Converts a rotation with roll, pitch, and yaw into a quaternion
+ static Eigen::Quaterniond EulerAnglesToQuaternion(const Eigen::Vector3d &rpy);
+ // Inverse of above function
+ static Eigen::Vector3d QuaternionToEulerAngles(const Eigen::Quaterniond &q);
+ // Converts a 3d rotation matrix into a rotation with roll, pitch, and yaw
+ static Eigen::Vector3d RotationMatrixToEulerAngles(const Eigen::Matrix3d &R);
+
+ // Builds a TargetPoseFbs from a TargetPose
+ static flatbuffers::Offset<TargetPoseFbs> TargetPoseToFbs(
+ const TargetMapper::TargetPose &target_pose,
+ flatbuffers::FlatBufferBuilder *fbb);
+ // Converts a TargetPoseFbs to a TargetPose
+ static TargetMapper::TargetPose TargetPoseFromFbs(
+ const TargetPoseFbs &target_pose_fbs);
};
// Transforms robot position and target detection data into target constraints
-// to be used for mapping. Interpolates continous-time data, converting it to
-// discrete detection time steps.
+// to be used for mapping.
class DataAdapter {
public:
- // Pairs pose with a time point
- struct TimestampedPose {
- aos::distributed_clock::time_point time;
- ceres::examples::Pose2d pose;
- };
-
// Pairs target detection with a time point
struct TimestampedDetection {
aos::distributed_clock::time_point time;
@@ -107,58 +114,38 @@
TargetMapper::TargetId id;
};
- // Pairs consecutive target detections into constraints, and interpolates
- // robot poses based on time points to compute motion between detections. This
- // prepares data to be used by TargetMapper. Also returns vector of delta
- // robot poses corresponding to each constraint, to be used for testing.
- //
- // Assumes both inputs are in chronological order.
- static std::pair<std::vector<ceres::examples::Constraint2d>,
- std::vector<ceres::examples::Pose2d>>
- MatchTargetDetections(
- const std::vector<TimestampedPose> ×tamped_robot_poses,
- const std::vector<TimestampedDetection> ×tamped_target_detections);
-
// Pairs consecutive target detections that are not too far apart in time into
// constraints. Meant to be used on a system without a position measurement.
// Assumes timestamped_target_detections is in chronological order.
// max_dt is the maximum time between two target detections to match them up.
// If too much time passes, the recoding device (box of pis) could have moved
// too much
- static std::vector<ceres::examples::Constraint2d> MatchTargetDetections(
+ static ceres::examples::VectorOfConstraints MatchTargetDetections(
const std::vector<TimestampedDetection> ×tamped_target_detections,
aos::distributed_clock::duration max_dt = std::chrono::milliseconds(1));
// Computes inverse of covariance matrix, assuming there was a target
// detection between robot movement over the given time period. Ceres calls
// this matrix the "information"
- static Eigen::Matrix3d ComputeConfidence(
+ static TargetMapper::ConfidenceMatrix ComputeConfidence(
aos::distributed_clock::time_point start,
aos::distributed_clock::time_point end, double distance_from_camera_start,
double distance_from_camera_end);
private:
- static ceres::examples::Pose2d InterpolatePose(
- const TimestampedPose &pose_start, const TimestampedPose &pose_end,
- aos::distributed_clock::time_point time);
-
// Computes the constraint between the start and end pose of the targets: the
// relative pose between the start and end target locations in the frame of
- // the start target. Takes into account the robot motion in the time between
- // the two detections.
- static ceres::examples::Constraint2d ComputeTargetConstraint(
- const TimestampedDetection &target_detection_start,
- const Eigen::Affine3d &H_robotstart_robotend,
- const TimestampedDetection &target_detection_end,
- const Eigen::Matrix3d &confidence);
- // Same as above function, but assumes no robot motion between the two
- // detections
- static ceres::examples::Constraint2d ComputeTargetConstraint(
+ // the start target.
+ static ceres::examples::Constraint3d ComputeTargetConstraint(
const TimestampedDetection &target_detection_start,
const TimestampedDetection &target_detection_end,
- const Eigen::Matrix3d &confidence);
+ const TargetMapper::ConfidenceMatrix &confidence);
};
+std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose);
+std::ostream &operator<<(std::ostream &os,
+ ceres::examples::Constraint3d constraint);
+
} // namespace frc971::vision
#endif // FRC971_VISION_TARGET_MAPPER_H_
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index ab06c5b..271f787 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -3,7 +3,9 @@
#include <random>
#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
#include "aos/testing/random_seed.h"
+#include "aos/util/math.h"
#include "glog/logging.h"
#include "gtest/gtest.h"
@@ -12,46 +14,65 @@
namespace {
constexpr double kToleranceMeters = 0.05;
constexpr double kToleranceRadians = 0.05;
+// Conversions between euler angles and quaternion result in slightly-off
+// doubles
+constexpr double kOrientationEqTolerance = 1e-10;
+constexpr std::chrono::milliseconds kMaxDt = std::chrono::milliseconds(3);
constexpr std::string_view kFieldName = "test";
} // namespace
-#define EXPECT_POSE_NEAR(pose1, pose2) \
- EXPECT_NEAR(pose1.x, pose2.x, kToleranceMeters); \
- EXPECT_NEAR(pose1.y, pose2.y, kToleranceMeters); \
- EXPECT_NEAR(pose1.yaw_radians, pose2.yaw_radians, kToleranceRadians);
-
-#define EXPECT_POSE_EQ(pose1, pose2) \
- EXPECT_DOUBLE_EQ(pose1.x, pose2.x); \
- EXPECT_DOUBLE_EQ(pose1.y, pose2.y); \
- EXPECT_DOUBLE_EQ(pose1.yaw_radians, pose2.yaw_radians);
-
-#define EXPECT_BETWEEN_EXCLUSIVE(value, a, b) \
- { \
- auto low = std::min(a, b); \
- auto high = std::max(a, b); \
- EXPECT_GT(value, low); \
- EXPECT_LT(value, high); \
+// Angles normalized by aos::math::NormalizeAngle()
+#define EXPECT_NORMALIZED_ANGLES_NEAR(theta1, theta2, tolerance) \
+ { \
+ double delta = std::abs(aos::math::DiffAngle(theta1, theta2)); \
+ /* Have to check delta - 2pi for the case that one angle is very */ \
+ /* close to -pi, and the other is very close to +pi */ \
+ EXPECT_TRUE(delta < tolerance || std::abs(aos::math::DiffAngle( \
+ delta, 2.0 * M_PI)) < tolerance); \
}
-namespace {
-// Expects angles to be normalized
-double DeltaAngle(double a, double b) {
- double delta = std::abs(a - b);
- return std::min(delta, (2.0 * M_PI) - delta);
-}
-} // namespace
+#define EXPECT_POSE_NEAR(pose1, pose2) \
+ { \
+ for (size_t i = 0; i < 3; i++) { \
+ EXPECT_NEAR(pose1.p(i), pose2.p(i), kToleranceMeters); \
+ } \
+ auto rpy_1 = PoseUtils::QuaternionToEulerAngles(pose1.q); \
+ auto rpy_2 = PoseUtils::QuaternionToEulerAngles(pose2.q); \
+ for (size_t i = 0; i < 3; i++) { \
+ SCOPED_TRACE(absl::StrFormat("rpy_1(%d) = %f, rpy_2(%d) = %f", i, \
+ rpy_1(i), i, rpy_2(i))); \
+ EXPECT_NORMALIZED_ANGLES_NEAR(rpy_1(i), rpy_2(i), kToleranceRadians) \
+ } \
+ }
-// Expects angles to be normalized
-#define EXPECT_ANGLE_BETWEEN_EXCLUSIVE(theta, a, b) \
- EXPECT_LT(DeltaAngle(a, theta), DeltaAngle(a, b)); \
- EXPECT_LT(DeltaAngle(b, theta), DeltaAngle(a, b));
+#define EXPECT_POSE_EQ(pose1, pose2) \
+ EXPECT_EQ(pose1.p, pose2.p); \
+ EXPECT_EQ(pose1.q, pose2.q);
-#define EXPECT_POSE_IN_RANGE(interpolated_pose, pose_start, pose_end) \
- EXPECT_BETWEEN_EXCLUSIVE(interpolated_pose.x, pose_start.x, pose_end.x); \
- EXPECT_BETWEEN_EXCLUSIVE(interpolated_pose.y, pose_start.y, pose_end.y); \
- EXPECT_ANGLE_BETWEEN_EXCLUSIVE(interpolated_pose.yaw_radians, \
- pose_start.yaw_radians, \
- pose_end.yaw_radians);
+#define EXPECT_QUATERNION_NEAR(q1, q2) \
+ EXPECT_NEAR(q1.x(), q2.x(), kOrientationEqTolerance) << q1 << " != " << q2; \
+ EXPECT_NEAR(q1.y(), q2.y(), kOrientationEqTolerance) << q1 << " != " << q2; \
+ EXPECT_NEAR(q1.z(), q2.z(), kOrientationEqTolerance) << q1 << " != " << q2; \
+ EXPECT_NEAR(q1.w(), q2.w(), kOrientationEqTolerance) << q1 << " != " << q2;
+
+// Expects same roll, pitch, and yaw values (not equivalent rotation)
+#define EXPECT_RPY_EQ(rpy_1, rpy_2) \
+ { \
+ for (size_t i = 0; i < 3; i++) { \
+ SCOPED_TRACE(absl::StrFormat("rpy_1(%d) = %f, rpy_2(%d) = %f", i, \
+ rpy_1(i), i, rpy_2(i))); \
+ EXPECT_NORMALIZED_ANGLES_NEAR(rpy_1(i), rpy_2(i), \
+ kOrientationEqTolerance) \
+ } \
+ }
+
+#define EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(roll, pitch, yaw) \
+ { \
+ auto rpy = Eigen::Vector3d(roll, pitch, yaw); \
+ auto converted_rpy = PoseUtils::QuaternionToEulerAngles( \
+ PoseUtils::EulerAnglesToQuaternion(rpy)); \
+ EXPECT_RPY_EQ(converted_rpy, rpy); \
+ }
// Both confidence matrixes should have the same dimensions and be square
#define EXPECT_CONFIDENCE_GT(confidence1, confidence2) \
@@ -65,18 +86,21 @@
}
namespace {
-ceres::examples::Pose2d MakePose(double x, double y, double yaw_radians) {
- return ceres::examples::Pose2d{x, y, yaw_radians};
+ceres::examples::Pose3d Make2dPose(double x, double y, double yaw_radians) {
+ return ceres::examples::Pose3d{Eigen::Vector3d(x, y, 0.0),
+ PoseUtils::EulerAnglesToQuaternion(
+ Eigen::Vector3d(0.0, 0.0, yaw_radians))};
}
// Assumes camera and robot origin are the same
DataAdapter::TimestampedDetection MakeTimestampedDetection(
aos::distributed_clock::time_point time, Eigen::Affine3d H_robot_target,
TargetMapper::TargetId id) {
- auto target_pose = PoseUtils::Affine3dToPose2d(H_robot_target);
+ auto target_pose = PoseUtils::Affine3dToPose3d(H_robot_target);
return DataAdapter::TimestampedDetection{
time, H_robot_target,
- std::sqrt(std::pow(target_pose.x, 2) + std::pow(target_pose.y, 2)), id};
+ std::sqrt(std::pow(target_pose.p(0), 2) + std::pow(target_pose.p(1), 2)),
+ id};
}
bool TargetIsInView(TargetMapper::TargetPose target_detection) {
@@ -84,14 +108,14 @@
// camera, assuming camera is pointing in the
// positive x-direction of the robot
double angle_to_target =
- atan2(target_detection.pose.y, target_detection.pose.x);
+ atan2(target_detection.pose.p(1), target_detection.pose.p(0));
// Simulated camera field of view, in radians
constexpr double kCameraFov = M_PI_2;
- if (fabs(angle_to_target) <= kCameraFov / 2.0) {
- VLOG(2) << "Found target in view, based on T = " << target_detection.pose.x
- << ", " << target_detection.pose.y << " with angle "
- << angle_to_target;
+ if (std::abs(angle_to_target) <= kCameraFov / 2.0) {
+ VLOG(2) << "Found target in view, based on T = "
+ << target_detection.pose.p(0) << ", " << target_detection.pose.p(1)
+ << " with angle " << angle_to_target;
return true;
} else {
return false;
@@ -104,6 +128,44 @@
} // namespace
+TEST(PoseUtilsTest, EulerAnglesAndQuaternionConversions) {
+ // Make sure that the conversions are consistent back and forth.
+ // These angles shouldn't get changed to a different, equivalent roll pitch
+ // yaw.
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, M_PI_2);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, -M_PI_2);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, 0.0, 0.0);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, M_PI_4, 0.0);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, 0.0);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(0.0, -M_PI_4, M_PI_4);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(M_PI_4, -M_PI_4, M_PI_4);
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(-M_PI_2, -M_PI_4, M_PI_4);
+
+ // Now, do a sweep of roll, pitch, and yaws in the normalized
+ // range.
+ // - roll: (-pi/2, pi/2)
+ // - pitch: (-pi/2, pi/2)
+ // - yaw: [-pi, pi)
+ constexpr double kThetaMaxRoll = M_PI_2 - kToleranceRadians;
+ constexpr double kThetaMaxPitch = M_PI_2 - kToleranceRadians;
+ constexpr double kThetaMaxYaw = M_PI;
+ constexpr double kDeltaTheta = M_PI / 16;
+
+ for (double roll = -kThetaMaxRoll; roll < kThetaMaxRoll;
+ roll += kDeltaTheta) {
+ for (double pitch = -kThetaMaxPitch; pitch < kThetaMaxPitch;
+ pitch += kDeltaTheta) {
+ for (double yaw = -kThetaMaxYaw; yaw < kThetaMaxYaw; yaw += kDeltaTheta) {
+ SCOPED_TRACE(
+ absl::StrFormat("roll: %f, pitch: %f, yaw: %f", roll, pitch, yaw));
+ EXPECT_EULER_ANGLES_QUATERNION_BACK_AND_FORTH_EQ(roll, pitch, yaw);
+ }
+ }
+ }
+}
+
TEST(DataAdapterTest, ComputeConfidence) {
// Check the confidence matrices. Don't check the actual values
// in case the constants change, just check that the confidence of contraints
@@ -113,10 +175,12 @@
constexpr double kDistanceStart = 0.5;
constexpr double kDistanceEnd = 2.0;
- Eigen::Matrix3d last_confidence = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix last_confidence =
+ TargetMapper::ConfidenceMatrix::Zero();
for (size_t dt = 0; dt < 15; dt++) {
- Eigen::Matrix3d confidence = DataAdapter::ComputeConfidence(
- TimeInMs(0), TimeInMs(dt), kDistanceStart, kDistanceEnd);
+ TargetMapper::ConfidenceMatrix confidence =
+ DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(dt),
+ kDistanceStart, kDistanceEnd);
if (dt != 0) {
// Confidence only decreases every 5ms (control loop period)
@@ -134,11 +198,13 @@
// Vary distance at start
constexpr int kDt = 3;
constexpr double kDistanceEnd = 1.5;
- Eigen::Matrix3d last_confidence = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix last_confidence =
+ TargetMapper::ConfidenceMatrix::Zero();
for (double distance_start = 0.0; distance_start < 3.0;
distance_start += 0.5) {
- Eigen::Matrix3d confidence = DataAdapter::ComputeConfidence(
- TimeInMs(0), TimeInMs(kDt), distance_start, kDistanceEnd);
+ TargetMapper::ConfidenceMatrix confidence =
+ DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(kDt),
+ distance_start, kDistanceEnd);
if (distance_start != 0.0) {
EXPECT_CONFIDENCE_GT(last_confidence, confidence);
}
@@ -150,10 +216,12 @@
// Vary distance at end
constexpr int kDt = 2;
constexpr double kDistanceStart = 2.5;
- Eigen::Matrix3d last_confidence = Eigen::Matrix3d::Zero();
+ TargetMapper::ConfidenceMatrix last_confidence =
+ TargetMapper::ConfidenceMatrix::Zero();
for (double distance_end = 0.0; distance_end < 3.0; distance_end += 0.5) {
- Eigen::Matrix3d confidence = DataAdapter::ComputeConfidence(
- TimeInMs(0), TimeInMs(kDt), kDistanceStart, distance_end);
+ TargetMapper::ConfidenceMatrix confidence =
+ DataAdapter::ComputeConfidence(TimeInMs(0), TimeInMs(kDt),
+ kDistanceStart, distance_end);
if (distance_end != 0.0) {
EXPECT_CONFIDENCE_GT(last_confidence, confidence);
}
@@ -163,103 +231,23 @@
}
TEST(DataAdapterTest, MatchTargetDetections) {
- std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
- {TimeInMs(0), ceres::examples::Pose2d{1.0, 2.0, 0.0}},
- {TimeInMs(5), ceres::examples::Pose2d{1.0, 2.0, 0.0}},
- {TimeInMs(10), ceres::examples::Pose2d{3.0, 1.0, M_PI_2}},
- {TimeInMs(15), ceres::examples::Pose2d{5.0, -2.0, -M_PI}},
- {TimeInMs(20), ceres::examples::Pose2d{5.0, -2.0, -M_PI}},
- {TimeInMs(25), ceres::examples::Pose2d{10.0, -32.0, M_PI_2}},
- {TimeInMs(30), ceres::examples::Pose2d{-15.0, 12.0, 0.0}},
- {TimeInMs(35), ceres::examples::Pose2d{-15.0, 12.0, 0.0}}};
- std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
- {{TimeInMs(5),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
- 1.0, 0},
- {TimeInMs(9),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
- 1.0, 1},
- {TimeInMs(9),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
- 1.0, 2},
- {TimeInMs(15),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
- 1.0, 0},
- {TimeInMs(16),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
- 1.0, 2},
- {TimeInMs(27),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -4.0, 0.0}),
- 1.0, 1}};
- auto [target_constraints, robot_delta_poses] =
- DataAdapter::MatchTargetDetections(timestamped_robot_poses,
- timestamped_target_detections);
-
- // Check that target constraints got inserted in the
- // correct spots
- EXPECT_EQ(target_constraints.size(),
- timestamped_target_detections.size() - 1);
- for (auto it = target_constraints.begin(); it < target_constraints.end();
- it++) {
- auto timestamped_it = timestamped_target_detections.begin() +
- (it - target_constraints.begin());
- EXPECT_EQ(it->id_begin, timestamped_it->id);
- EXPECT_EQ(it->id_end, (timestamped_it + 1)->id);
- }
-
- // Check that poses were interpolated correctly.
- // Keep track of the computed robot pose by adding the delta poses
- auto computed_robot_pose = timestamped_robot_poses[1].pose;
-
- computed_robot_pose =
- PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[0]);
- EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[1].pose,
- timestamped_robot_poses[2].pose);
-
- computed_robot_pose =
- PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[1]);
- EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[1].pose,
- timestamped_robot_poses[2].pose);
- EXPECT_POSE_EQ(robot_delta_poses[1], MakePose(0.0, 0.0, 0.0));
-
- computed_robot_pose =
- PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[2]);
- EXPECT_POSE_EQ(computed_robot_pose, timestamped_robot_poses[3].pose);
-
- computed_robot_pose =
- PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[3]);
- EXPECT_POSE_EQ(computed_robot_pose, timestamped_robot_poses[4].pose);
-
- computed_robot_pose =
- PoseUtils::ComputeOffsetPose(computed_robot_pose, robot_delta_poses[4]);
- EXPECT_POSE_IN_RANGE(computed_robot_pose, timestamped_robot_poses[5].pose,
- timestamped_robot_poses[6].pose);
-}
-
-TEST(DataAdapterTest, MatchTargetDetectionsWithoutRobotPosition) {
std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
{MakeTimestampedDetection(
- TimeInMs(5),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{5.0, -5.0, 0.0}),
+ TimeInMs(5), PoseUtils::Pose3dToAffine3d(Make2dPose(5.0, -5.0, 0.0)),
2),
- MakeTimestampedDetection(TimeInMs(6),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{5.0, -4.0, M_PI}),
- 0),
- MakeTimestampedDetection(TimeInMs(10),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{3.0, -3.0, M_PI}),
- 1),
- MakeTimestampedDetection(TimeInMs(13),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{4.0, -7.0, M_PI_2}),
- 2),
- MakeTimestampedDetection(TimeInMs(14),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{4.0, -4.0, M_PI_2}),
- 2)};
+ MakeTimestampedDetection(
+ TimeInMs(6),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(5.0, -4.0, M_PI)), 0),
+ MakeTimestampedDetection(
+ TimeInMs(10),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(3.0, -3.0, M_PI)), 1),
+ MakeTimestampedDetection(
+ TimeInMs(13),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(4.0, -7.0, M_PI_2)), 2),
+ MakeTimestampedDetection(
+ TimeInMs(14),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(4.0, -4.0, M_PI_2)), 2)};
- constexpr auto kMaxDt = std::chrono::milliseconds(3);
auto target_constraints =
DataAdapter::MatchTargetDetections(timestamped_target_detections, kMaxDt);
@@ -271,201 +259,219 @@
timestamped_target_detections.size() - 3);
// Between 5ms and 6ms detections
- EXPECT_DOUBLE_EQ(target_constraints[0].x, 0.0);
- EXPECT_DOUBLE_EQ(target_constraints[0].y, 1.0);
- EXPECT_DOUBLE_EQ(target_constraints[0].yaw_radians, -M_PI);
+ EXPECT_DOUBLE_EQ(target_constraints[0].t_be.p(0), 0.0);
+ EXPECT_DOUBLE_EQ(target_constraints[0].t_be.p(1), 1.0);
+ EXPECT_QUATERNION_NEAR(
+ target_constraints[0].t_be.q,
+ PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d(0.0, 0.0, M_PI)));
EXPECT_EQ(target_constraints[0].id_begin, 2);
EXPECT_EQ(target_constraints[0].id_end, 0);
// Between 10ms and 13ms detections
- EXPECT_DOUBLE_EQ(target_constraints[1].x, -1.0);
- EXPECT_DOUBLE_EQ(target_constraints[1].y, 4.0);
- EXPECT_DOUBLE_EQ(target_constraints[1].yaw_radians, -M_PI_2);
+ EXPECT_DOUBLE_EQ(target_constraints[1].t_be.p(0), -1.0);
+ EXPECT_DOUBLE_EQ(target_constraints[1].t_be.p(1), 4.0);
+ EXPECT_QUATERNION_NEAR(
+ target_constraints[1].t_be.q,
+ PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d(0.0, 0.0, -M_PI_2)));
EXPECT_EQ(target_constraints[1].id_begin, 1);
EXPECT_EQ(target_constraints[1].id_end, 2);
}
TEST(TargetMapperTest, TwoTargetsOneConstraint) {
- std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
- target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
- target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, 0.0};
+ ceres::examples::MapOfPoses target_poses;
+ target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
+ target_poses[1] = Make2dPose(-5.0, 0.0, 0.0);
- std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
- {TimeInMs(5), ceres::examples::Pose2d{2.0, 0.0, 0.0}},
- {TimeInMs(10), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
- {TimeInMs(15), ceres::examples::Pose2d{-1.0, 0.0, 0.0}}};
std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
{MakeTimestampedDetection(
- TimeInMs(5),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{3.0, 0.0, M_PI}),
+ TimeInMs(5), PoseUtils::Pose3dToAffine3d(Make2dPose(3.0, 0.0, M_PI)),
0),
MakeTimestampedDetection(
- TimeInMs(10),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{-4.0, 0.0, 0.0}),
+ TimeInMs(6), PoseUtils::Pose3dToAffine3d(Make2dPose(-7.0, 0.0, 0.0)),
1)};
auto target_constraints =
- DataAdapter::MatchTargetDetections(timestamped_robot_poses,
- timestamped_target_detections)
- .first;
+ DataAdapter::MatchTargetDetections(timestamped_target_detections, kMaxDt);
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
mapper.Solve(kFieldName);
ASSERT_EQ(mapper.target_poses().size(), 2);
- EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
- EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, 0.0));
+ EXPECT_POSE_NEAR(mapper.target_poses()[0], Make2dPose(5.0, 0.0, M_PI));
+ EXPECT_POSE_NEAR(mapper.target_poses()[1], Make2dPose(-5.0, 0.0, 0.0));
}
TEST(TargetMapperTest, TwoTargetsTwoConstraints) {
- std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
- target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
- target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, -M_PI_2};
+ ceres::examples::MapOfPoses target_poses;
+ target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
+ target_poses[1] = Make2dPose(-5.0, 0.0, -M_PI_2);
- std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
- {TimeInMs(5), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
- {TimeInMs(10), ceres::examples::Pose2d{3.0, 0.0, 0.0}},
- {TimeInMs(15), ceres::examples::Pose2d{4.0, 0.0, 0.0}},
- {TimeInMs(20), ceres::examples::Pose2d{-1.0, 0.0, 0.0}}};
std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
{MakeTimestampedDetection(
TimeInMs(5),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{6.0, 0.0, M_PI}),
- 0),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(6.0, 0.0, M_PI_2)), 0),
MakeTimestampedDetection(
- TimeInMs(10),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{-8.0, 0.0, -M_PI_2}),
- 1),
+ TimeInMs(7),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(6.0, 10.0, -M_PI)), 1),
MakeTimestampedDetection(
- TimeInMs(15),
- PoseUtils::Pose2dToAffine3d(ceres::examples::Pose2d{1.0, 0.0, M_PI}),
- 0)};
+ TimeInMs(12),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(1.0, 0.0, M_PI)), 0),
+ MakeTimestampedDetection(
+ TimeInMs(13),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(-9.0, 0.0, -M_PI_2)), 1)};
auto target_constraints =
- DataAdapter::MatchTargetDetections(timestamped_robot_poses,
- timestamped_target_detections)
- .first;
+ DataAdapter::MatchTargetDetections(timestamped_target_detections, kMaxDt);
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
mapper.Solve(kFieldName);
ASSERT_EQ(mapper.target_poses().size(), 2);
- EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
- EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, -M_PI_2));
+ EXPECT_POSE_NEAR(mapper.target_poses()[0], Make2dPose(5.0, 0.0, M_PI));
+ EXPECT_POSE_NEAR(mapper.target_poses()[1], Make2dPose(-5.0, 0.0, -M_PI_2));
}
TEST(TargetMapperTest, TwoTargetsOneNoisyConstraint) {
- std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
- target_poses[0] = ceres::examples::Pose2d{5.0, 0.0, M_PI};
- target_poses[1] = ceres::examples::Pose2d{-5.0, 0.0, 0.0};
+ ceres::examples::MapOfPoses target_poses;
+ target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
+ target_poses[1] = Make2dPose(-5.0, 0.0, 0.0);
- std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses = {
- {TimeInMs(5), ceres::examples::Pose2d{1.99, 0.0, 0.0}},
- {TimeInMs(10), ceres::examples::Pose2d{-1.0, 0.0, 0.0}},
- {TimeInMs(15), ceres::examples::Pose2d{-1.01, -0.01, 0.004}}};
std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections =
{MakeTimestampedDetection(
TimeInMs(5),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{3.01, 0.001, M_PI - 0.001}),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(3.01, 0.001, M_PI - 0.001)),
0),
- MakeTimestampedDetection(TimeInMs(10),
- PoseUtils::Pose2dToAffine3d(
- ceres::examples::Pose2d{-4.01, 0.0, 0.0}),
- 1)};
+ MakeTimestampedDetection(
+ TimeInMs(7),
+ PoseUtils::Pose3dToAffine3d(Make2dPose(-7.01, 0.0, 0.0)), 1)};
auto target_constraints =
- DataAdapter::MatchTargetDetections(timestamped_robot_poses,
- timestamped_target_detections)
- .first;
+ DataAdapter::MatchTargetDetections(timestamped_target_detections, kMaxDt);
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
mapper.Solve(kFieldName);
ASSERT_EQ(mapper.target_poses().size(), 2);
- EXPECT_POSE_NEAR(mapper.target_poses()[0], MakePose(5.0, 0.0, M_PI));
- EXPECT_POSE_NEAR(mapper.target_poses()[1], MakePose(-5.0, 0.0, 0.0));
+ EXPECT_POSE_NEAR(mapper.target_poses()[0], Make2dPose(5.0, 0.0, M_PI));
+ EXPECT_POSE_NEAR(mapper.target_poses()[1], Make2dPose(-5.0, 0.0, 0.0));
}
-TEST(TargetMapperTest, MultiTargetCircleMotion) {
- // Build set of target locations wrt global origin
- // For simplicity, do this on a grid of the field
- double field_half_length = 7.5; // half length of the field
- double field_half_width = 5.0; // half width of the field
- std::map<TargetMapper::TargetId, ceres::examples::Pose2d> target_poses;
- std::vector<TargetMapper::TargetPose> actual_target_poses;
- for (int i = 0; i < 3; i++) {
- for (int j = 0; j < 3; j++) {
- TargetMapper::TargetId target_id = i * 3 + j;
- TargetMapper::TargetPose target_pose{
- target_id, ceres::examples::Pose2d{field_half_length * (1 - i),
- field_half_width * (1 - j), 0.0}};
- actual_target_poses.emplace_back(target_pose);
- target_poses[target_id] = target_pose.pose;
- VLOG(2) << "VERTEX_SE2 " << target_id << " " << target_pose.pose.x << " "
- << target_pose.pose.y << " " << target_pose.pose.yaw_radians;
+class ChargedUpTargetMapperTest : public ::testing::Test {
+ public:
+ ChargedUpTargetMapperTest() : generator_(aos::testing::RandomSeed()) {}
+
+ // Generates noisy target detection if target in camera FOV
+ std::optional<DataAdapter::TimestampedDetection> MaybeGenerateNoisyDetection(
+ const ceres::examples::Pose3d &robot_pose,
+ const TargetMapper::TargetPose &target_pose, size_t t,
+ bool force_in_view = false) {
+ TargetMapper::TargetPose target_detection = {
+ .id = target_pose.id,
+ .pose = PoseUtils::ComputeRelativePose(robot_pose, target_pose.pose)};
+ if (force_in_view || TargetIsInView(target_detection)) {
+ // Define random generator with Gaussian
+ // distribution
+ constexpr double kMean = 0.0;
+ constexpr double kStdDev = 1.0;
+ // Can play with this to see how it impacts
+ // randomness
+ constexpr double kNoiseScalePosition = 0.01;
+ constexpr double kNoiseScaleOrientation = 0.0005;
+ std::normal_distribution<double> dist(kMean, kStdDev);
+
+ target_detection.pose.p(0) += dist(generator_) * kNoiseScalePosition;
+ target_detection.pose.p(1) += dist(generator_) * kNoiseScalePosition;
+ target_detection.pose.p(2) += dist(generator_) * kNoiseScalePosition;
+
+ target_detection.pose.q.w() += dist(generator_) * kNoiseScaleOrientation;
+ target_detection.pose.q.x() += dist(generator_) * kNoiseScaleOrientation;
+ target_detection.pose.q.y() += dist(generator_) * kNoiseScaleOrientation;
+ target_detection.pose.q.z() += dist(generator_) * kNoiseScaleOrientation;
+
+ auto time_point = TimeInMs(t);
+ return MakeTimestampedDetection(
+ time_point, PoseUtils::Pose3dToAffine3d(target_detection.pose),
+ target_detection.id);
}
+
+ return std::nullopt;
}
+ private:
+ std::default_random_engine generator_;
+};
+
+// Drive in a circle around the 2023 field, and add a bit of randomness to 3d
+// pose detections
+TEST_F(ChargedUpTargetMapperTest, FieldCircleMotion) {
+ // Read target map
+ auto target_map_fbs = aos::JsonFileToFlatbuffer<TargetMap>(
+ aos::testing::ArtifactPath("frc971/vision/target_map.json"));
+
+ std::vector<TargetMapper::TargetPose> actual_target_poses;
+ ceres::examples::MapOfPoses target_poses;
+ for (const auto *target_pose_fbs : *target_map_fbs.message().target_poses()) {
+ const TargetMapper::TargetPose target_pose =
+ PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+ actual_target_poses.emplace_back(target_pose);
+ target_poses[target_pose.id] = target_pose.pose;
+ }
+
+ double kFieldHalfLength = 16.54 / 2.0; // half length of the field
+ double kFieldHalfWidth = 8.02 / 2.0; // half width of the field
+
// Now, create a bunch of robot poses and target
// observations
- size_t dt = 1;
+ constexpr size_t kDt = 5;
+ constexpr double kRobotZ = 1.0;
- std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses;
std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections;
constexpr size_t kTotalSteps = 100;
for (size_t step_count = 0; step_count < kTotalSteps; step_count++) {
- size_t t = dt * step_count;
+ size_t t = kDt * step_count;
+
// Circle clockwise around the center of the field
double robot_theta = t;
- double robot_x = (field_half_length / 2.0) * cos(robot_theta);
- double robot_y = (-field_half_width / 2.0) * sin(robot_theta);
- ceres::examples::Pose2d robot_pose{robot_x, robot_y, robot_theta};
+ constexpr double kRadiusScalar = 0.5;
+ double robot_x = (kRadiusScalar * kFieldHalfLength) * cos(robot_theta);
+ double robot_y = (kRadiusScalar * -kFieldHalfWidth) * sin(robot_theta);
+
+ auto robot_pose =
+ ceres::examples::Pose3d{.p = Eigen::Vector3d(robot_x, robot_y, kRobotZ),
+ .q = PoseUtils::EulerAnglesToQuaternion(
+ Eigen::Vector3d(robot_theta, 0.0, 0.0))};
+
for (TargetMapper::TargetPose target_pose : actual_target_poses) {
- TargetMapper::TargetPose target_detection = {
- .id = target_pose.id,
- .pose = PoseUtils::ComputeRelativePose(robot_pose, target_pose.pose)};
- if (TargetIsInView(target_detection)) {
- // Define random generator with Gaussian
- // distribution
- const double mean = 0.0;
- const double stddev = 1.0;
- // Can play with this to see how it impacts
- // randomness
- constexpr double kNoiseScale = 0.01;
- std::default_random_engine generator(aos::testing::RandomSeed());
- std::normal_distribution<double> dist(mean, stddev);
-
- target_detection.pose.x += dist(generator) * kNoiseScale;
- target_detection.pose.y += dist(generator) * kNoiseScale;
- robot_pose.x += dist(generator) * kNoiseScale;
- robot_pose.y += dist(generator) * kNoiseScale;
-
- auto time_point =
- aos::distributed_clock::time_point(std::chrono::milliseconds(t));
- timestamped_robot_poses.emplace_back(DataAdapter::TimestampedPose{
- .time = time_point, .pose = robot_pose});
- timestamped_target_detections.emplace_back(MakeTimestampedDetection(
- time_point, PoseUtils::Pose2dToAffine3d(target_detection.pose),
- target_detection.id));
+ auto optional_detection =
+ MaybeGenerateNoisyDetection(robot_pose, target_pose, t);
+ if (optional_detection.has_value()) {
+ timestamped_target_detections.emplace_back(*optional_detection);
}
}
}
+ // The above circular motion only captures targets 1-4, so add another
+ // detection with the camera looking at targets 5-8, and 4 at the same time to
+ // have a connection to the rest of the targets
{
- // Add in a robot pose after all target poses
- auto final_robot_pose =
- timestamped_robot_poses[timestamped_robot_poses.size() - 1];
- timestamped_robot_poses.emplace_back(DataAdapter::TimestampedPose{
- .time = final_robot_pose.time + std::chrono::milliseconds(dt),
- .pose = final_robot_pose.pose});
+ auto last_robot_pose =
+ ceres::examples::Pose3d{.p = Eigen::Vector3d(0.0, 0.0, kRobotZ),
+ .q = PoseUtils::EulerAnglesToQuaternion(
+ Eigen::Vector3d(0.0, 0.0, M_PI))};
+ for (size_t id = 4; id <= 8; id++) {
+ auto target_pose =
+ TargetMapper::GetTargetPoseById(actual_target_poses, id).value();
+ auto optional_detection = MaybeGenerateNoisyDetection(
+ last_robot_pose, target_pose, kTotalSteps * kDt, true);
+
+ ASSERT_TRUE(optional_detection.has_value())
+ << "Didn't detect target " << target_pose.id;
+ timestamped_target_detections.emplace_back(*optional_detection);
+ }
}
auto target_constraints =
- DataAdapter::MatchTargetDetections(timestamped_robot_poses,
- timestamped_target_detections)
- .first;
+ DataAdapter::MatchTargetDetections(timestamped_target_detections, kMaxDt);
frc971::vision::TargetMapper mapper(target_poses, target_constraints);
mapper.Solve(kFieldName);
@@ -483,8 +489,10 @@
for (auto [target_id, target_pose] : target_poses) {
// Skip first pose, since that needs to be correct
// and is fixed in the solver
- if (target_id != 0) {
- ceres::examples::Pose2d bad_pose{0.0, 0.0, M_PI / 2.0};
+ if (target_id != 1) {
+ ceres::examples::Pose3d bad_pose{
+ Eigen::Vector3d::Zero(),
+ PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d::Zero())};
target_poses[target_id] = bad_pose;
}
}
diff --git a/frc971/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
index 053bfa1..2553003 100644
--- a/frc971/vision/v4l2_reader.cc
+++ b/frc971/vision/v4l2_reader.cc
@@ -85,18 +85,23 @@
for (size_t i = 0; i < buffers_.size(); ++i) {
buffers_[i].sender = event_loop_->MakeSender<CameraImage>("/camera");
- EnqueueBuffer(i);
+ MarkBufferToBeEnqueued(i);
}
int type = multiplanar() ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE
: V4L2_BUF_TYPE_VIDEO_CAPTURE;
PCHECK(Ioctl(VIDIOC_STREAMON, &type) == 0);
}
+void V4L2ReaderBase::MarkBufferToBeEnqueued(int buffer_index) {
+ ReinitializeBuffer(buffer_index);
+ EnqueueBuffer(buffer_index);
+}
+
void V4L2ReaderBase::MaybeEnqueue() {
// First, enqueue any old buffer we already have. This is the one which
// may have been sent.
if (saved_buffer_) {
- EnqueueBuffer(saved_buffer_.index);
+ MarkBufferToBeEnqueued(saved_buffer_.index);
saved_buffer_.Clear();
}
ftrace_.FormatMessage("Enqueued previous buffer %d", saved_buffer_.index);
@@ -114,7 +119,7 @@
// going.
if (previous_buffer) {
ftrace_.FormatMessage("Previous %d", previous_buffer.index);
- EnqueueBuffer(previous_buffer.index);
+ MarkBufferToBeEnqueued(previous_buffer.index);
}
continue;
}
@@ -133,7 +138,12 @@
}
}
-void V4L2ReaderBase::SendLatestImage() { buffers_[saved_buffer_.index].Send(); }
+void V4L2ReaderBase::SendLatestImage() {
+ buffers_[saved_buffer_.index].Send();
+
+ MarkBufferToBeEnqueued(saved_buffer_.index);
+ saved_buffer_.Clear();
+}
void V4L2ReaderBase::SetExposure(size_t duration) {
v4l2_control manual_control;
@@ -236,7 +246,8 @@
CHECK_GE(buffer_number, 0);
CHECK_LT(buffer_number, static_cast<int>(buffers_.size()));
- buffers_[buffer_number].InitializeMessage(ImageSize());
+ CHECK(buffers_[buffer_number].data_pointer != nullptr);
+
struct v4l2_buffer buffer;
struct v4l2_plane planes[1];
memset(&buffer, 0, sizeof(buffer));
@@ -315,16 +326,22 @@
const std::string &image_sensor_subdev)
: V4L2ReaderBase(event_loop, device_name),
epoll_(epoll),
- image_sensor_fd_(open(image_sensor_subdev.c_str(), O_RDWR | O_NONBLOCK)) {
+ image_sensor_fd_(open(image_sensor_subdev.c_str(), O_RDWR | O_NONBLOCK)),
+ buffer_requeuer_([this](int buffer) { EnqueueBuffer(buffer); },
+ kEnqueueFifoPriority) {
PCHECK(image_sensor_fd_.get() != -1)
<< " Failed to open device " << device_name;
-
StreamOn();
epoll_->OnReadable(fd().get(), [this]() { OnImageReady(); });
}
RockchipV4L2Reader::~RockchipV4L2Reader() { epoll_->DeleteFd(fd().get()); }
+void RockchipV4L2Reader::MarkBufferToBeEnqueued(int buffer) {
+ ReinitializeBuffer(buffer);
+ buffer_requeuer_.Push(buffer);
+}
+
void RockchipV4L2Reader::OnImageReady() {
if (!ReadLatestImage()) {
return;
@@ -366,16 +383,19 @@
PCHECK(ImageSensorIoctl(VIDIOC_S_EXT_CTRLS, &controls) == 0);
}
-void RockchipV4L2Reader::SetBlanking(size_t hblank, size_t vblank) {
- v4l2_control hblank_control;
- hblank_control.id = V4L2_CID_HBLANK;
- hblank_control.value = static_cast<int>(hblank);
- PCHECK(ImageSensorIoctl(VIDIOC_S_CTRL, &hblank_control) == 0);
+void RockchipV4L2Reader::SetVerticalBlanking(size_t vblank) {
+ struct v4l2_ext_controls controls;
+ memset(&controls, 0, sizeof(controls));
+ struct v4l2_ext_control control[1];
+ memset(&control, 0, sizeof(control));
- v4l2_control vblank_control;
- vblank_control.id = V4L2_CID_VBLANK;
- vblank_control.value = static_cast<int>(vblank);
- PCHECK(ImageSensorIoctl(VIDIOC_S_CTRL, &vblank_control) == 0);
+ controls.ctrl_class = V4L2_CTRL_CLASS_IMAGE_SOURCE;
+ controls.count = 1;
+ controls.controls = control;
+ control[0].id = V4L2_CID_VBLANK;
+ control[0].value = vblank;
+
+ PCHECK(ImageSensorIoctl(VIDIOC_S_EXT_CTRLS, &controls) == 0);
}
} // namespace vision
diff --git a/frc971/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
index e36fef8..22e4367 100644
--- a/frc971/vision/v4l2_reader.h
+++ b/frc971/vision/v4l2_reader.h
@@ -5,10 +5,13 @@
#include <string>
#include "absl/types/span.h"
+#include "aos/containers/ring_buffer.h"
#include "aos/events/epoll.h"
#include "aos/events/event_loop.h"
#include "aos/ftrace.h"
+#include "aos/realtime.h"
#include "aos/scoped/scoped_fd.h"
+#include "aos/util/threaded_consumer.h"
#include "frc971/vision/vision_generated.h"
#include "glog/logging.h"
@@ -57,6 +60,23 @@
void StreamOff();
void StreamOn();
+ // Enqueues a buffer for v4l2 to stream into (expensive).
+ void EnqueueBuffer(int buffer_index);
+
+ // Initializations that need to happen in the main thread.
+ //
+ // Implementations of MarkBufferToBeEnqueued should call this before calling
+ // EnqueueBuffer.
+ void ReinitializeBuffer(int buffer_index) {
+ CHECK_GE(buffer_index, 0);
+ CHECK_LT(buffer_index, static_cast<int>(buffers_.size()));
+ buffers_[buffer_index].InitializeMessage(ImageSize());
+ }
+
+ // Submits a buffer to be enqueued later in a lower priority thread.
+ // Legacy V4L2Reader still does this in the main thread.
+ virtual void MarkBufferToBeEnqueued(int buffer_index);
+
int Ioctl(unsigned long number, void *arg);
bool multiplanar() const { return multiplanar_; }
@@ -70,9 +90,9 @@
const aos::ScopedFD &fd() { return fd_; };
- private:
static constexpr int kNumberBuffers = 4;
+ private:
struct Buffer {
void InitializeMessage(size_t max_image_size);
@@ -80,7 +100,7 @@
aos::monotonic_clock::time_point monotonic_eof);
void Send() {
- (void)builder.Send(message_offset);
+ builder.CheckOk(builder.Send(message_offset));
message_offset = flatbuffers::Offset<CameraImage>();
}
@@ -113,8 +133,6 @@
// buffer, or BufferInfo() if there wasn't a frame to dequeue.
BufferInfo DequeueBuffer();
- void EnqueueBuffer(int buffer);
-
// The mmaped V4L2 buffers.
std::array<Buffer, kNumberBuffers> buffers_;
@@ -154,16 +172,22 @@
void SetGain(size_t gain);
void SetGainExt(size_t gain);
- void SetBlanking(size_t hblank, size_t vblank);
+ void SetVerticalBlanking(size_t vblank);
private:
void OnImageReady();
+ void MarkBufferToBeEnqueued(int buffer) override;
+
int ImageSensorIoctl(unsigned long number, void *arg);
aos::internal::EPoll *epoll_;
aos::ScopedFD image_sensor_fd_;
+
+ static constexpr int kEnqueueFifoPriority = 1;
+
+ aos::util::ThreadedConsumer<int, kNumberBuffers> buffer_requeuer_;
};
} // namespace vision
diff --git a/scouting/db/db.go b/scouting/db/db.go
index 121ba52..8f043de 100644
--- a/scouting/db/db.go
+++ b/scouting/db/db.go
@@ -184,7 +184,7 @@
}
func (database *Database) AddToStats(s Stats) error {
- matches, err := database.QueryMatches(s.TeamNumber)
+ matches, err := database.queryMatches(s.TeamNumber)
if err != nil {
return err
}
@@ -304,7 +304,7 @@
return rankins, result.Error
}
-func (database *Database) QueryMatches(teamNumber_ int32) ([]Match, error) {
+func (database *Database) queryMatches(teamNumber_ int32) ([]Match, error) {
var matches []Match
result := database.
Where("r1 = $1 OR r2 = $1 OR r3 = $1 OR b1 = $1 OR b2 = $1 OR b3 = $1", teamNumber_).
diff --git a/scouting/db/db_test.go b/scouting/db/db_test.go
index 642fb6d..9e85651 100644
--- a/scouting/db/db_test.go
+++ b/scouting/db/db_test.go
@@ -234,41 +234,6 @@
}
}
-func TestQueryMatchDB(t *testing.T) {
- fixture := createDatabase(t)
- defer fixture.TearDown()
-
- testDatabase := []Match{
- Match{MatchNumber: 2, SetNumber: 1, CompLevel: "quals", R1: 251, R2: 169, R3: 286, B1: 253, B2: 538, B3: 149},
- Match{MatchNumber: 4, SetNumber: 1, CompLevel: "quals", R1: 198, R2: 135, R3: 777, B1: 999, B2: 434, B3: 698},
- Match{MatchNumber: 3, SetNumber: 1, CompLevel: "quals", R1: 147, R2: 421, R3: 538, B1: 126, B2: 448, B3: 262},
- Match{MatchNumber: 6, SetNumber: 1, CompLevel: "quals", R1: 191, R2: 132, R3: 773, B1: 994, B2: 435, B3: 696},
- }
-
- for i := 0; i < len(testDatabase); i++ {
- err := fixture.db.AddToMatch(testDatabase[i])
- check(t, err, fmt.Sprint("Failed to add match", i))
- }
-
- correct := []Match{
- Match{
- MatchNumber: 2, SetNumber: 1, CompLevel: "quals",
- R1: 251, R2: 169, R3: 286, B1: 253, B2: 538, B3: 149,
- },
- Match{
- MatchNumber: 3, SetNumber: 1, CompLevel: "quals",
- R1: 147, R2: 421, R3: 538, B1: 126, B2: 448, B3: 262,
- },
- }
-
- got, err := fixture.db.QueryMatches(538)
- check(t, err, "Failed to query matches for 538")
-
- if !reflect.DeepEqual(correct, got) {
- t.Fatalf("Got %#v,\nbut expected %#v.", got, correct)
- }
-}
-
func TestQueryShiftDB(t *testing.T) {
fixture := createDatabase(t)
defer fixture.TearDown()
diff --git a/scouting/webserver/requests/BUILD b/scouting/webserver/requests/BUILD
index 1d90a78..9eb207d 100644
--- a/scouting/webserver/requests/BUILD
+++ b/scouting/webserver/requests/BUILD
@@ -20,8 +20,6 @@
"//scouting/webserver/requests/messages:request_all_notes_response_go_fbs",
"//scouting/webserver/requests/messages:request_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:request_data_scouting_response_go_fbs",
- "//scouting/webserver/requests/messages:request_matches_for_team_go_fbs",
- "//scouting/webserver/requests/messages:request_matches_for_team_response_go_fbs",
"//scouting/webserver/requests/messages:request_notes_for_team_go_fbs",
"//scouting/webserver/requests/messages:request_notes_for_team_response_go_fbs",
"//scouting/webserver/requests/messages:request_shift_schedule_go_fbs",
@@ -61,8 +59,6 @@
"//scouting/webserver/requests/messages:request_all_notes_response_go_fbs",
"//scouting/webserver/requests/messages:request_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:request_data_scouting_response_go_fbs",
- "//scouting/webserver/requests/messages:request_matches_for_team_go_fbs",
- "//scouting/webserver/requests/messages:request_matches_for_team_response_go_fbs",
"//scouting/webserver/requests/messages:request_notes_for_team_go_fbs",
"//scouting/webserver/requests/messages:request_shift_schedule_go_fbs",
"//scouting/webserver/requests/messages:request_shift_schedule_response_go_fbs",
diff --git a/scouting/webserver/requests/debug/BUILD b/scouting/webserver/requests/debug/BUILD
index 454d030..650ba82 100644
--- a/scouting/webserver/requests/debug/BUILD
+++ b/scouting/webserver/requests/debug/BUILD
@@ -13,7 +13,6 @@
"//scouting/webserver/requests/messages:request_all_matches_response_go_fbs",
"//scouting/webserver/requests/messages:request_all_notes_response_go_fbs",
"//scouting/webserver/requests/messages:request_data_scouting_response_go_fbs",
- "//scouting/webserver/requests/messages:request_matches_for_team_response_go_fbs",
"//scouting/webserver/requests/messages:request_notes_for_team_response_go_fbs",
"//scouting/webserver/requests/messages:request_shift_schedule_response_go_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_response_go_fbs",
diff --git a/scouting/webserver/requests/debug/cli/cli_test.py b/scouting/webserver/requests/debug/cli/cli_test.py
index 5cfeb61..6d33082 100644
--- a/scouting/webserver/requests/debug/cli/cli_test.py
+++ b/scouting/webserver/requests/debug/cli/cli_test.py
@@ -222,23 +222,6 @@
stdout)
self.assertEqual(stdout.count("MatchNumber:"), 90)
- def test_request_matches_for_team(self):
- self.refresh_match_list()
-
- json_path = write_json_request({
- "team": 4856,
- })
- exit_code, stdout, stderr = run_debug_cli(
- ["-requestMatchesForTeam", json_path])
-
- # Team 4856 has 12 matches.
- self.assertEqual(exit_code, 0, stderr)
- self.assertIn(
- "MatchList: ([]*request_matches_for_team_response.MatchT) (len=12 cap=12) {",
- stdout)
- self.assertEqual(stdout.count("MatchNumber:"), 12)
- self.assertEqual(len(re.findall(r": \(int32\) 4856[,\n]", stdout)), 12)
-
def test_request_all_matches(self):
"""Makes sure that we can import the match list multiple times without problems."""
request_all_matches_outputs = []
diff --git a/scouting/webserver/requests/debug/cli/main.go b/scouting/webserver/requests/debug/cli/main.go
index ec38270..066dec5 100644
--- a/scouting/webserver/requests/debug/cli/main.go
+++ b/scouting/webserver/requests/debug/cli/main.go
@@ -91,8 +91,6 @@
"If specified, parse the file as a submitNotes JSON request.")
requestAllMatchesPtr := flag.String("requestAllMatches", "",
"If specified, parse the file as a RequestAllMatches JSON request.")
- requestMatchesForTeamPtr := flag.String("requestMatchesForTeam", "",
- "If specified, parse the file as a RequestMatchesForTeam JSON request.")
requestDataScoutingPtr := flag.String("requestDataScouting", "",
"If specified, parse the file as a RequestDataScouting JSON request.")
requestAllDriverRankingsPtr := flag.String("requestAllDriverRankings", "",
@@ -138,13 +136,6 @@
debug.RequestAllMatches)
maybePerformRequest(
- "RequestMatchesForTeam",
- "scouting/webserver/requests/messages/request_matches_for_team.fbs",
- *requestMatchesForTeamPtr,
- *addressPtr,
- debug.RequestMatchesForTeam)
-
- maybePerformRequest(
"RequestDataScouting",
"scouting/webserver/requests/messages/request_data_scouting.fbs",
*requestDataScoutingPtr,
diff --git a/scouting/webserver/requests/debug/debug.go b/scouting/webserver/requests/debug/debug.go
index 08cf67c..130850c 100644
--- a/scouting/webserver/requests/debug/debug.go
+++ b/scouting/webserver/requests/debug/debug.go
@@ -15,7 +15,6 @@
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_matches_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_notes_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_data_scouting_response"
- "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_shift_schedule_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting_response"
@@ -125,12 +124,6 @@
request_all_driver_rankings_response.GetRootAsRequestAllDriverRankingsResponse)
}
-func RequestMatchesForTeam(server string, requestBytes []byte) (*request_matches_for_team_response.RequestMatchesForTeamResponseT, error) {
- return sendMessage[request_matches_for_team_response.RequestMatchesForTeamResponseT](
- server+"/requests/request/matches_for_team", requestBytes,
- request_matches_for_team_response.GetRootAsRequestMatchesForTeamResponse)
-}
-
func RequestDataScouting(server string, requestBytes []byte) (*request_data_scouting_response.RequestDataScoutingResponseT, error) {
return sendMessage[request_data_scouting_response.RequestDataScoutingResponseT](
server+"/requests/request/data_scouting", requestBytes,
diff --git a/scouting/webserver/requests/messages/BUILD b/scouting/webserver/requests/messages/BUILD
index fcda246..2e118d5 100644
--- a/scouting/webserver/requests/messages/BUILD
+++ b/scouting/webserver/requests/messages/BUILD
@@ -11,8 +11,6 @@
"request_all_matches_response",
"request_all_notes",
"request_all_notes_response",
- "request_matches_for_team",
- "request_matches_for_team_response",
"request_data_scouting",
"request_data_scouting_response",
"refresh_match_list",
diff --git a/scouting/webserver/requests/messages/request_matches_for_team.fbs b/scouting/webserver/requests/messages/request_matches_for_team.fbs
deleted file mode 100644
index dd1b217..0000000
--- a/scouting/webserver/requests/messages/request_matches_for_team.fbs
+++ /dev/null
@@ -1,7 +0,0 @@
-namespace scouting.webserver.requests;
-
-table RequestMatchesForTeam {
- team:int (id: 0);
-}
-
-root_type RequestMatchesForTeam;
diff --git a/scouting/webserver/requests/messages/request_matches_for_team_response.fbs b/scouting/webserver/requests/messages/request_matches_for_team_response.fbs
deleted file mode 100644
index dcfec52..0000000
--- a/scouting/webserver/requests/messages/request_matches_for_team_response.fbs
+++ /dev/null
@@ -1,20 +0,0 @@
-namespace scouting.webserver.requests;
-
-table Match {
- match_number:int (id: 0);
- set_number:int (id: 1);
- comp_level:string (id: 2);
- r1:int (id: 3);
- r2:int (id: 4);
- r3:int (id: 5);
- b1:int (id: 6);
- b2:int (id: 7);
- b3:int (id: 8);
-}
-//TODO(Sabina): de-duplicate the Match struct in request_all_matches
-
-table RequestMatchesForTeamResponse {
- match_list:[Match] (id:0);
-}
-
-root_type RequestMatchesForTeamResponse;
diff --git a/scouting/webserver/requests/requests.go b/scouting/webserver/requests/requests.go
index 11fd317..99b5459 100644
--- a/scouting/webserver/requests/requests.go
+++ b/scouting/webserver/requests/requests.go
@@ -23,8 +23,6 @@
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_notes_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_data_scouting_response"
- "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team"
- "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_shift_schedule"
@@ -51,8 +49,6 @@
type RequestAllDriverRankingsResponseT = request_all_driver_rankings_response.RequestAllDriverRankingsResponseT
type RequestAllNotes = request_all_notes.RequestAllNotes
type RequestAllNotesResponseT = request_all_notes_response.RequestAllNotesResponseT
-type RequestMatchesForTeam = request_matches_for_team.RequestMatchesForTeam
-type RequestMatchesForTeamResponseT = request_matches_for_team_response.RequestMatchesForTeamResponseT
type RequestDataScouting = request_data_scouting.RequestDataScouting
type RequestDataScoutingResponseT = request_data_scouting_response.RequestDataScoutingResponseT
type RefreshMatchList = refresh_match_list.RefreshMatchList
@@ -81,7 +77,6 @@
ReturnAllDriverRankings() ([]db.DriverRankingData, error)
ReturnAllShifts() ([]db.Shift, error)
ReturnStats() ([]db.Stats, error)
- QueryMatches(int32) ([]db.Match, error)
QueryAllShifts(int) ([]db.Shift, error)
QueryStats(int) ([]db.Stats, error)
QueryNotes(int32) ([]string, error)
@@ -255,49 +250,6 @@
w.Write(builder.FinishedBytes())
}
-// Handles a RequestMatchesForTeam request.
-type requestMatchesForTeamHandler struct {
- db Database
-}
-
-func (handler requestMatchesForTeamHandler) ServeHTTP(w http.ResponseWriter, req *http.Request) {
- requestBytes, err := io.ReadAll(req.Body)
- if err != nil {
- respondWithError(w, http.StatusBadRequest, fmt.Sprint("Failed to read request bytes:", err))
- return
- }
-
- request, success := parseRequest(w, requestBytes, "RequestMatchesForTeam", request_matches_for_team.GetRootAsRequestMatchesForTeam)
- if !success {
- return
- }
-
- matches, err := handler.db.QueryMatches(request.Team())
- if err != nil {
- respondWithError(w, http.StatusInternalServerError, fmt.Sprint("Faled to query database: ", err))
- return
- }
-
- var response RequestAllMatchesResponseT
- for _, match := range matches {
- response.MatchList = append(response.MatchList, &request_all_matches_response.MatchT{
- MatchNumber: match.MatchNumber,
- SetNumber: match.SetNumber,
- CompLevel: match.CompLevel,
- R1: match.R1,
- R2: match.R2,
- R3: match.R3,
- B1: match.B1,
- B2: match.B2,
- B3: match.B3,
- })
- }
-
- builder := flatbuffers.NewBuilder(50 * 1024)
- builder.Finish((&response).Pack(builder))
- w.Write(builder.FinishedBytes())
-}
-
// Handles a RequestDataScouting request.
type requestDataScoutingHandler struct {
db Database
@@ -745,7 +697,6 @@
scoutingServer.Handle("/requests/request/all_matches", requestAllMatchesHandler{db})
scoutingServer.Handle("/requests/request/all_notes", requestAllNotesHandler{db})
scoutingServer.Handle("/requests/request/all_driver_rankings", requestAllDriverRankingsHandler{db})
- scoutingServer.Handle("/requests/request/matches_for_team", requestMatchesForTeamHandler{db})
scoutingServer.Handle("/requests/request/data_scouting", requestDataScoutingHandler{db})
scoutingServer.Handle("/requests/refresh_match_list", refreshMatchListHandler{db, scrape})
scoutingServer.Handle("/requests/submit/submit_notes", submitNoteScoutingHandler{db})
diff --git a/scouting/webserver/requests/requests_test.go b/scouting/webserver/requests/requests_test.go
index ed8a3b5..9775eae 100644
--- a/scouting/webserver/requests/requests_test.go
+++ b/scouting/webserver/requests/requests_test.go
@@ -21,8 +21,6 @@
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_all_notes_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_data_scouting_response"
- "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team"
- "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_matches_for_team_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_notes_for_team"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_shift_schedule"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_shift_schedule_response"
@@ -184,55 +182,6 @@
}
-// Validates that we can request the full match list.
-func TestRequestMatchesForTeam(t *testing.T) {
- db := MockDatabase{
- matches: []db.Match{
- {
- MatchNumber: 1, SetNumber: 1, CompLevel: "qual",
- R1: 5, R2: 42, R3: 600, B1: 971, B2: 400, B3: 200,
- },
- {
- MatchNumber: 2, SetNumber: 1, CompLevel: "qual",
- R1: 6, R2: 43, R3: 601, B1: 972, B2: 401, B3: 201,
- },
- },
- }
- scoutingServer := server.NewScoutingServer()
- HandleRequests(&db, scrapeEmtpyMatchList, scoutingServer)
- scoutingServer.Start(8080)
- defer scoutingServer.Stop()
-
- builder := flatbuffers.NewBuilder(1024)
- builder.Finish((&request_matches_for_team.RequestMatchesForTeamT{
- Team: 971,
- }).Pack(builder))
-
- response, err := debug.RequestMatchesForTeam("http://localhost:8080", builder.FinishedBytes())
- if err != nil {
- t.Fatal("Failed to request all matches: ", err)
- }
-
- expected := request_matches_for_team_response.RequestMatchesForTeamResponseT{
- MatchList: []*request_matches_for_team_response.MatchT{
- // MatchNumber, SetNumber, CompLevel
- // R1, R2, R3, B1, B2, B3
- {
- 1, 1, "qual",
- 5, 42, 600, 971, 400, 200,
- },
- },
- }
- if len(expected.MatchList) != len(response.MatchList) {
- t.Fatal("Expected ", expected, ", but got ", *response)
- }
- for i, match := range expected.MatchList {
- if !reflect.DeepEqual(*match, *response.MatchList[i]) {
- t.Fatal("Expected for match", i, ":", *match, ", but got:", *response.MatchList[i])
- }
- }
-}
-
// Validates that we can request the stats.
func TestRequestDataScouting(t *testing.T) {
db := MockDatabase{
@@ -753,19 +702,6 @@
return database.stats, nil
}
-func (database *MockDatabase) QueryMatches(requestedTeam int32) ([]db.Match, error) {
- var matches []db.Match
- for _, match := range database.matches {
- for _, team := range []int32{match.R1, match.R2, match.R3, match.B1, match.B2, match.B3} {
- if team == requestedTeam {
- matches = append(matches, match)
- break
- }
- }
- }
- return matches, nil
-}
-
func (database *MockDatabase) QueryStats(int) ([]db.Stats, error) {
return []db.Stats{}, nil
}
diff --git a/third_party/apriltag/BUILD b/third_party/apriltag/BUILD
new file mode 100644
index 0000000..b11e34f
--- /dev/null
+++ b/third_party/apriltag/BUILD
@@ -0,0 +1,93 @@
+load("@//tools/build_rules:select.bzl", "compiler_select")
+
+cc_library(
+ name = "apriltag",
+ srcs = [
+ "apriltag.c",
+ "apriltag_pose.c",
+ "apriltag_quad_thresh.c",
+ "common/g2d.c",
+ "common/getopt.c",
+ "common/homography.c",
+ "common/image_u8.c",
+ "common/image_u8x3.c",
+ "common/image_u8x4.c",
+ "common/matd.c",
+ "common/pam.c",
+ "common/pjpeg.c",
+ "common/pjpeg-idct.c",
+ "common/pnm.c",
+ "common/string_util.c",
+ "common/svd22.c",
+ "common/time_util.c",
+ "common/unionfind.c",
+ "common/workerpool.c",
+ "common/zarray.c",
+ "common/zhash.c",
+ "common/zmaxheap.c",
+ "tag16h5.c",
+ "tag25h9.c",
+ "tag36h11.c",
+ "tagCircle21h7.c",
+ "tagCircle49h12.c",
+ "tagCustom48h12.c",
+ "tagStandard41h12.c",
+ "tagStandard52h13.c",
+ ],
+ hdrs = [
+ "apriltag.h",
+ "apriltag_math.h",
+ "apriltag_pose.h",
+ "common/debug_print.h",
+ "common/g2d.h",
+ "common/getopt.h",
+ "common/homography.h",
+ "common/image_types.h",
+ "common/image_u8.h",
+ "common/image_u8x3.h",
+ "common/image_u8x4.h",
+ "common/matd.h",
+ "common/math_util.h",
+ "common/pam.h",
+ "common/pjpeg.h",
+ "common/pnm.h",
+ "common/postscript_utils.h",
+ "common/pthreads_cross.h",
+ "common/string_util.h",
+ "common/svd22.h",
+ "common/time_util.h",
+ "common/timeprofile.h",
+ "common/unionfind.h",
+ "common/workerpool.h",
+ "common/zarray.h",
+ "common/zhash.h",
+ "common/zmaxheap.h",
+ "tag16h5.h",
+ "tag25h9.h",
+ "tag36h11.h",
+ "tagCircle21h7.h",
+ "tagCircle49h12.h",
+ "tagCustom48h12.h",
+ "tagStandard41h12.h",
+ "tagStandard52h13.h",
+ ],
+ copts = compiler_select({
+ "clang": [
+ "-Wno-cast-align",
+ "-Wno-incompatible-pointer-types-discards-qualifiers",
+ ],
+ "gcc": [
+ "-Wno-discarded-qualifiers",
+ "-Wno-shift-negative-value",
+ ],
+ }) + [
+ "-Wno-sign-compare",
+ "-Wno-cast-qual",
+ "-Wno-unused-parameter",
+ "-Wno-unused-variable",
+ "-Wno-format-nonliteral",
+ ],
+ includes = ["."],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/third_party/apriltag/common/workerpool.c b/third_party/apriltag/common/workerpool.c
index a0170ef..29eccfc 100644
--- a/third_party/apriltag/common/workerpool.c
+++ b/third_party/apriltag/common/workerpool.c
@@ -41,20 +41,6 @@
#include "workerpool.h"
#include "debug_print.h"
-struct workerpool {
- int nthreads;
- zarray_t *tasks;
- int taskspos;
-
- pthread_t *threads;
- int *status;
-
- pthread_mutex_t mutex;
- pthread_cond_t startcond; // used to signal the availability of work
- pthread_cond_t endcond; // used to signal completion of all work
-
- int end_count; // how many threads are done?
-};
struct task
{
diff --git a/third_party/apriltag/common/workerpool.h b/third_party/apriltag/common/workerpool.h
index 2c32ab1..a233b5b 100644
--- a/third_party/apriltag/common/workerpool.h
+++ b/third_party/apriltag/common/workerpool.h
@@ -31,6 +31,21 @@
typedef struct workerpool workerpool_t;
+struct workerpool {
+ int nthreads;
+ zarray_t *tasks;
+ int taskspos;
+
+ pthread_t *threads;
+ int *status;
+
+ pthread_mutex_t mutex;
+ pthread_cond_t startcond; // used to signal the availability of work
+ pthread_cond_t endcond; // used to signal completion of all work
+
+ int end_count; // how many threads are done?
+};
+
// as a special case, if nthreads==1, no additional threads are
// created, and workerpool_run will run synchronously.
workerpool_t *workerpool_create(int nthreads);
@@ -41,7 +56,8 @@
// runs all added tasks, waits for them to complete.
void workerpool_run(workerpool_t *wp);
-// same as workerpool_run, except always single threaded. (mostly for debugging).
+// same as workerpool_run, except always single threaded. (mostly for
+// debugging).
void workerpool_run_single(workerpool_t *wp);
int workerpool_get_nthreads(workerpool_t *wp);
diff --git a/third_party/foxglove_ws_protocol/BUILD b/third_party/foxglove/BUILD
similarity index 100%
copy from third_party/foxglove_ws_protocol/BUILD
copy to third_party/foxglove/BUILD
diff --git a/third_party/foxglove_ws_protocol/BUILD b/third_party/foxglove/schemas/BUILD
similarity index 100%
copy from third_party/foxglove_ws_protocol/BUILD
copy to third_party/foxglove/schemas/BUILD
diff --git a/third_party/foxglove/schemas/schemas.BUILD b/third_party/foxglove/schemas/schemas.BUILD
new file mode 100644
index 0000000..a1a5d29
--- /dev/null
+++ b/third_party/foxglove/schemas/schemas.BUILD
@@ -0,0 +1,21 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "DEFAULT_FLATC_ARGS")
+
+FLATC_ARGS = [arg for arg in DEFAULT_FLATC_ARGS if arg != "--require-explicit-ids"]
+
+flatbuffer_cc_library(
+ name = "schemas",
+ srcs = glob(["*.fbs"]),
+ flatc_args = FLATC_ARGS,
+ gen_reflections = True,
+ visibility = ["//visibility:public"],
+)
+
+load("@org_frc971//aos:flatbuffers.bzl", "cc_static_flatbuffer")
+
+[cc_static_flatbuffer(
+ name = filename[:-4] + "_schema",
+ bfbs_name = filename[:-4] + ".bfbs",
+ function = "foxglove::" + filename[:-4] + "Schema",
+ target = ":schemas_reflection_out",
+ visibility = ["//visibility:public"],
+) for filename in glob(["*.fbs"])]
diff --git a/third_party/foxglove_ws_protocol/BUILD b/third_party/foxglove/ws_protocol/BUILD
similarity index 100%
rename from third_party/foxglove_ws_protocol/BUILD
rename to third_party/foxglove/ws_protocol/BUILD
diff --git a/third_party/foxglove_ws_protocol/foxglove_ws_protocol.BUILD b/third_party/foxglove/ws_protocol/foxglove_ws_protocol.BUILD
similarity index 100%
rename from third_party/foxglove_ws_protocol/foxglove_ws_protocol.BUILD
rename to third_party/foxglove/ws_protocol/foxglove_ws_protocol.BUILD
diff --git a/third_party/foxglove_ws_protocol/foxglove_ws_protocol.patch b/third_party/foxglove/ws_protocol/foxglove_ws_protocol.patch
similarity index 100%
rename from third_party/foxglove_ws_protocol/foxglove_ws_protocol.patch
rename to third_party/foxglove/ws_protocol/foxglove_ws_protocol.patch
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index 879cd0f..c55ccc9 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -4,9 +4,9 @@
#include <cmath>
#include <memory>
-#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/drivetrain/camera.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+#include "frc971/control_loops/pose.h"
namespace y2019 {
namespace control_loops {
@@ -42,7 +42,9 @@
&dt_config,
Pose *robot_pose)
: ::frc971::control_loops::drivetrain::HybridEkf<Scalar>(dt_config),
- robot_pose_(robot_pose) {}
+ robot_pose_(robot_pose),
+ h_queue_(this),
+ make_h_queue_(this) {}
// Performs a kalman filter correction with a single camera frame, consisting
// of up to max_targets_per_frame targets and taken at time t.
@@ -79,26 +81,92 @@
// As such, we need to store the EKF functions that the remaining targets
// will need in arrays:
::aos::SizedArray<::std::function<Output(const State &, const Input &)>,
- max_targets_per_frame> h_functions;
+ max_targets_per_frame>
+ h_functions;
::aos::SizedArray<::std::function<Eigen::Matrix<Scalar, kNOutputs,
kNStates>(const State &)>,
- max_targets_per_frame> dhdx_functions;
- HybridEkf::Correct(
+ max_targets_per_frame>
+ dhdx_functions;
+ make_h_queue_.CorrectKnownHBuilder(
z, nullptr,
- ::std::bind(&TypedLocalizer::MakeH, this, camera, targets, &h_functions,
- &dhdx_functions, ::std::placeholders::_1,
- ::std::placeholders::_2, ::std::placeholders::_3,
- ::std::placeholders::_4),
- {}, {}, R, t);
+ ExpectedObservationBuilder(this, camera, targets, &h_functions,
+ &dhdx_functions),
+ R, t);
// Fetch cache:
for (size_t ii = 1; ii < targets.size(); ++ii) {
TargetViewToMatrices(targets[ii], &z, &R);
- HybridEkf::Correct(z, nullptr, {}, h_functions[ii], dhdx_functions[ii], R,
- t);
+ h_queue_.CorrectKnownH(
+ z, nullptr,
+ ExpectedObservationFunctor(h_functions[ii], dhdx_functions[ii]), R,
+ t);
}
}
private:
+ class ExpectedObservationFunctor
+ : public HybridEkf::ExpectedObservationFunctor {
+ public:
+ ExpectedObservationFunctor(
+ ::std::function<Output(const State &, const Input &)> h,
+ ::std::function<
+ Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+ dhdx)
+ : h_(h), dhdx_(dhdx) {}
+
+ Output H(const State &state, const Input &input) final {
+ return h_(state, input);
+ }
+
+ virtual Eigen::Matrix<Scalar, kNOutputs, kNStates> DHDX(
+ const State &state) final {
+ return dhdx_(state);
+ }
+
+ private:
+ ::std::function<Output(const State &, const Input &)> h_;
+ ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+ dhdx_;
+ };
+ class ExpectedObservationBuilder
+ : public HybridEkf::ExpectedObservationBuilder {
+ public:
+ ExpectedObservationBuilder(
+ TypedLocalizer *localizer, const Camera &camera,
+ const ::aos::SizedArray<TargetView, max_targets_per_frame>
+ &target_views,
+ ::aos::SizedArray<::std::function<Output(const State &, const Input &)>,
+ max_targets_per_frame> *h_functions,
+ ::aos::SizedArray<::std::function<Eigen::Matrix<
+ Scalar, kNOutputs, kNStates>(const State &)>,
+ max_targets_per_frame> *dhdx_functions)
+ : localizer_(localizer),
+ camera_(camera),
+ target_views_(target_views),
+ h_functions_(h_functions),
+ dhdx_functions_(dhdx_functions) {}
+
+ virtual ExpectedObservationFunctor *MakeExpectedObservations(
+ const State &state, const StateSquare &P) {
+ ::std::function<Output(const State &, const Input &)> h;
+ ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+ dhdx;
+ localizer_->MakeH(camera_, target_views_, h_functions_, dhdx_functions_,
+ state, P, &h, &dhdx);
+ functor_.emplace(h, dhdx);
+ return &functor_.value();
+ }
+
+ private:
+ TypedLocalizer *localizer_;
+ const Camera &camera_;
+ const ::aos::SizedArray<TargetView, max_targets_per_frame> &target_views_;
+ ::aos::SizedArray<::std::function<Output(const State &, const Input &)>,
+ max_targets_per_frame> *h_functions_;
+ ::aos::SizedArray<::std::function<Eigen::Matrix<Scalar, kNOutputs,
+ kNStates>(const State &)>,
+ max_targets_per_frame> *dhdx_functions_;
+ std::optional<ExpectedObservationFunctor> functor_;
+ };
// The threshold to use for completely rejecting potentially bad target
// matches.
// TODO(james): Tune
@@ -159,8 +227,8 @@
max_targets_per_frame> *dhdx_functions,
const State &X_hat, const StateSquare &P,
::std::function<Output(const State &, const Input &)> *h,
- ::std::function<
- Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)> *dhdx) {
+ ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+ *dhdx) {
// Because we need to match camera targets ("views") to actual field
// targets, and because we want to take advantage of the correlations
// between the targets (i.e., if we see two targets in the image, they
@@ -226,7 +294,8 @@
// as the scores matrix.
::std::array<::std::array<Eigen::Matrix<Scalar, kNOutputs, kNStates>,
max_targets_per_frame>,
- num_targets> all_H_matrices;
+ num_targets>
+ all_H_matrices;
// Iterate through and fill out the scores for each potential pairing:
for (size_t ii = 0; ii < target_views.size(); ++ii) {
@@ -360,8 +429,8 @@
}
}
- Eigen::Matrix<Scalar, kNOutputs, kNStates> HMatrix(
- const Target &target, const Pose &camera_pose) {
+ Eigen::Matrix<Scalar, kNOutputs, kNStates> HMatrix(const Target &target,
+ const Pose &camera_pose) {
// To calculate dheading/d{x,y,theta}:
// heading = arctan2(target_pos - camera_pos) - camera_theta
Eigen::Matrix<Scalar, 3, 1> target_pos = target.pose().abs_pos();
@@ -401,8 +470,8 @@
// the number of rows in scores/best_matches that are actually populated).
::std::array<int, max_targets_per_frame> MatchFrames(
const Eigen::Matrix<Scalar, max_targets_per_frame, num_targets> &scores,
- const Eigen::Matrix<int, max_targets_per_frame, max_targets_per_frame> &
- best_matches,
+ const Eigen::Matrix<int, max_targets_per_frame, max_targets_per_frame>
+ &best_matches,
int n_views) {
::std::array<int, max_targets_per_frame> best_set;
best_set.fill(-1);
@@ -424,8 +493,8 @@
// true, that means that we are done recursing.
void MatchFrames(
const Eigen::Matrix<Scalar, max_targets_per_frame, num_targets> &scores,
- const Eigen::Matrix<int, max_targets_per_frame, max_targets_per_frame> &
- best_matches,
+ const Eigen::Matrix<int, max_targets_per_frame, max_targets_per_frame>
+ &best_matches,
const ::aos::SizedArray<bool, max_targets_per_frame> &used_views,
const ::std::array<bool, num_targets> &used_targets,
::std::array<int, max_targets_per_frame> *best_set, Scalar *best_score) {
@@ -485,6 +554,15 @@
// The pose that is used by the cameras to determine the location of the robot
// and thus the expected view of the targets.
Pose *robot_pose_;
+
+ typename HybridEkf::template ExpectedObservationAllocator<
+ ExpectedObservationFunctor>
+ h_queue_;
+ typename HybridEkf::template ExpectedObservationAllocator<
+ ExpectedObservationBuilder>
+ make_h_queue_;
+
+ friend class ExpectedObservationBuilder;
}; // class TypedLocalizer
} // namespace control_loops
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 505a598..8f8bf87 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -89,6 +89,7 @@
: event_loop_(event_loop),
dt_config_(dt_config),
ekf_(dt_config),
+ observations_(&ekf_),
clock_offset_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/aos")),
@@ -244,7 +245,7 @@
rejection_reason = RejectionReason::IMAGE_FROM_FUTURE;
}
if (!result.has_camera_calibration()) {
- LOG(WARNING) << "Got camera frame without calibration data.";
+ AOS_LOG(WARNING, "Got camera frame without calibration data.\n");
ImageMatchDebug::Builder builder(*fbb);
builder.add_camera(camera_index);
builder.add_accepted(false);
@@ -401,11 +402,6 @@
Eigen::Matrix3f R = Eigen::Matrix3f::Zero();
R.diagonal() = noises.cwiseAbs2();
- Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> H;
- H.setZero();
- H(0, StateIdx::kX) = 1;
- H(1, StateIdx::kY) = 1;
- H(2, StateIdx::kTheta) = 1;
VLOG(1) << "Pose implied by target: " << Z.transpose()
<< " and current pose " << x() << ", " << y() << ", " << theta()
<< " Heading/dist/skew implied by target: "
@@ -447,47 +443,11 @@
// more convenient to write given the Correct() interface we already have.
// Note: If we start going back to doing back-in-time rewinds, then we can't
// get away with passing things by reference.
- ekf_.Correct(
- Eigen::Vector3f::Zero(), &U, {},
- [H, H_field_target, pose_robot_target, state_at_capture, Z,
- &correction_rejection](const State &,
- const Input &) -> Eigen::Vector3f {
- // Weighting for how much to use the current robot heading estimate
- // vs. the heading estimate from the image results. A value of 1.0
- // completely ignores the measured heading, but will prioritize turret
- // aiming above all else. A value of 0.0 will prioritize correcting
- // any gyro heading drift.
- constexpr float kImpliedWeight = 0.99;
- const float z_yaw_diff = aos::math::NormalizeAngle(
- state_at_capture.value()(Localizer::StateIdx::kTheta) - Z(2));
- const float z_yaw = Z(2) + kImpliedWeight * z_yaw_diff;
- const Eigen::Vector3f Z_implied =
- CalculateImpliedPose(z_yaw, H_field_target, pose_robot_target);
- const Eigen::Vector3f Z_used = Z_implied;
- // Just in case we ever do encounter any, drop measurements if they
- // have non-finite numbers.
- if (!Z.allFinite()) {
- AOS_LOG(WARNING, "Got measurement with infinites or NaNs.\n");
- correction_rejection = RejectionReason::NONFINITE_MEASUREMENT;
- return Eigen::Vector3f::Zero();
- }
- Eigen::Vector3f Zhat = H * state_at_capture.value() - Z_used;
- // Rewrap angle difference to put it back in range. Note that this
- // component of the error is currently ignored (see definition of H
- // above).
- Zhat(2) = aos::math::NormalizeAngle(Zhat(2));
- // If the measurement implies that we are too far from the current
- // estimate, then ignore it.
- // Note that I am not entirely sure how much effect this actually has,
- // because I primarily introduced it to make sure that any grossly
- // invalid measurements get thrown out.
- if (Zhat.squaredNorm() > std::pow(10.0, 2)) {
- correction_rejection = RejectionReason::CORRECTION_TOO_LARGE;
- return Eigen::Vector3f::Zero();
- }
- return Zhat;
- },
- [H](const State &) { return H; }, R, now);
+ observations_.CorrectKnownH(
+ Eigen::Vector3f::Zero(), &U,
+ Corrector(H_field_target, pose_robot_target, state_at_capture.value(),
+ Z, &correction_rejection),
+ R, now);
if (correction_rejection) {
builder.add_accepted(false);
builder.add_rejection_reason(*correction_rejection);
@@ -502,6 +462,43 @@
return debug_offsets;
}
+Localizer::Output Localizer::Corrector::H(const State &, const Input &) {
+ // Weighting for how much to use the current robot heading estimate
+ // vs. the heading estimate from the image results. A value of 1.0
+ // completely ignores the measured heading, but will prioritize turret
+ // aiming above all else. A value of 0.0 will prioritize correcting
+ // any gyro heading drift.
+ constexpr float kImpliedWeight = 0.99;
+ const float z_yaw_diff = aos::math::NormalizeAngle(
+ state_at_capture_(Localizer::StateIdx::kTheta) - Z_(2));
+ const float z_yaw = Z_(2) + kImpliedWeight * z_yaw_diff;
+ const Eigen::Vector3f Z_implied =
+ CalculateImpliedPose(z_yaw, H_field_target_, pose_robot_target_);
+ const Eigen::Vector3f Z_used = Z_implied;
+ // Just in case we ever do encounter any, drop measurements if they
+ // have non-finite numbers.
+ if (!Z_.allFinite()) {
+ AOS_LOG(WARNING, "Got measurement with infinites or NaNs.\n");
+ *correction_rejection_ = RejectionReason::NONFINITE_MEASUREMENT;
+ return Eigen::Vector3f::Zero();
+ }
+ Eigen::Vector3f Zhat = H_ * state_at_capture_ - Z_used;
+ // Rewrap angle difference to put it back in range. Note that this
+ // component of the error is currently ignored (see definition of H
+ // above).
+ Zhat(2) = aos::math::NormalizeAngle(Zhat(2));
+ // If the measurement implies that we are too far from the current
+ // estimate, then ignore it.
+ // Note that I am not entirely sure how much effect this actually has,
+ // because I primarily introduced it to make sure that any grossly
+ // invalid measurements get thrown out.
+ if (Zhat.squaredNorm() > std::pow(10.0, 2)) {
+ *correction_rejection_ = RejectionReason::CORRECTION_TOO_LARGE;
+ return Eigen::Vector3f::Zero();
+ }
+ return Zhat;
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace y2020
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index 5458797..de57091 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -9,8 +9,8 @@
#include "aos/network/message_bridge_server_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/drivetrain/localizer.h"
-#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
#include "y2020/control_loops/drivetrain/localizer_debug_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
#include "y2020/vision/sift/sift_generated.h"
namespace y2020 {
@@ -95,6 +95,37 @@
std::array<int, kNumRejectionReasons> rejection_counts;
};
+ class Corrector : public HybridEkf::ExpectedObservationFunctor {
+ public:
+ Corrector(const Eigen::Matrix<float, 4, 4> &H_field_target,
+ const Pose &pose_robot_target, const State &state_at_capture,
+ const Eigen::Vector3f &Z,
+ std::optional<RejectionReason> *correction_rejection)
+ : H_field_target_(H_field_target),
+ pose_robot_target_(pose_robot_target),
+ state_at_capture_(state_at_capture),
+ Z_(Z),
+ correction_rejection_(correction_rejection) {
+ H_.setZero();
+ H_(0, StateIdx::kX) = 1;
+ H_(1, StateIdx::kY) = 1;
+ H_(2, StateIdx::kTheta) = 1;
+ }
+ Output H(const State &, const Input &) final;
+ Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> DHDX(
+ const State &) final {
+ return H_;
+ }
+
+ private:
+ Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> H_;
+ const Eigen::Matrix<float, 4, 4> H_field_target_;
+ Pose pose_robot_target_;
+ const State state_at_capture_;
+ const Eigen::Vector3f &Z_;
+ std::optional<RejectionReason> *correction_rejection_;
+ };
+
// Processes new image data from the given pi and updates the EKF.
aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 5> HandleImageMatch(
size_t camera_index, std::string_view pi,
@@ -113,6 +144,7 @@
aos::EventLoop *const event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
HybridEkf ekf_;
+ HybridEkf::ExpectedObservationAllocator<Corrector> observations_;
std::vector<aos::Fetcher<frc971::vision::sift::ImageMatchResult>>
image_fetchers_;
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index 9e9e7dd..d280523 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -15,6 +15,7 @@
DEFINE_string(output_file, "",
"If set, logs all channels to the provided logfile.");
+DECLARE_bool(die_on_malloc);
// This file tests that the full 2020 localizer behaves sanely.
@@ -146,6 +147,7 @@
CHECK_EQ(aos::configuration::GetNodeIndex(configuration(), pi1_), 1);
set_team_id(frc971::control_loops::testing::kTeamNumber);
set_battery_voltage(12.0);
+ FLAGS_die_on_malloc = true;
if (!FLAGS_output_file.empty()) {
logger_event_loop_ = MakeEventLoop("logger", roborio_);
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 6145452..d5ed54f 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -53,7 +53,8 @@
[this](cv::Mat rgb_image,
const aos::monotonic_clock::time_point eof) {
charuco_extractor_.HandleImage(rgb_image, eof);
- }) {
+ },
+ std::chrono::milliseconds(5)) {
CHECK(pi_number_) << ": Invalid pi number " << pi
<< ", failed to parse pi number";
std::regex re{"^[0-9][0-9]-[0-9][0-9]"};
diff --git a/y2020/y2020_logger.json b/y2020/y2020_logger.json
index 451a2ee..911be81 100644
--- a/y2020/y2020_logger.json
+++ b/y2020/y2020_logger.json
@@ -190,7 +190,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "logger",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
diff --git a/y2020/y2020_pi_template.json b/y2020/y2020_pi_template.json
index 2b40728..cf54c23 100644
--- a/y2020/y2020_pi_template.json
+++ b/y2020/y2020_pi_template.json
@@ -199,7 +199,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "logger",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
@@ -249,7 +249,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "roborio",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
}
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index da52f28..eb97b1f 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -51,7 +51,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "roborio",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
diff --git a/y2022/control_loops/drivetrain/localizer.cc b/y2022/control_loops/drivetrain/localizer.cc
index 79ced2d..0aa4456 100644
--- a/y2022/control_loops/drivetrain/localizer.cc
+++ b/y2022/control_loops/drivetrain/localizer.cc
@@ -11,6 +11,7 @@
: event_loop_(event_loop),
dt_config_(dt_config),
ekf_(dt_config),
+ observations_(&ekf_),
localizer_output_fetcher_(
event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
"/localizer")),
@@ -48,7 +49,7 @@
joystick_state_fetcher_->autonomous()) {
// TODO(james): This is an inelegant way to avoid having the localizer mess
// up splines. Do better.
- //return;
+ // return;
}
if (localizer_output_fetcher_.Fetch()) {
clock_offset_fetcher_.Fetch();
@@ -89,11 +90,6 @@
}
}
- Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> H;
- H.setZero();
- H(0, StateIdx::kX) = 1;
- H(1, StateIdx::kY) = 1;
- H(2, StateIdx::kTheta) = 1;
const Eigen::Vector3f Z{
static_cast<float>(localizer_output_fetcher_->x()),
static_cast<float>(localizer_output_fetcher_->y()),
@@ -101,15 +97,8 @@
Eigen::Matrix3f R = Eigen::Matrix3f::Zero();
R.diagonal() << 0.01, 0.01, 1e-4;
const Input U_correct = ekf_.MostRecentInput();
- ekf_.Correct(
- Eigen::Vector3f::Zero(), &U_correct, {},
- [H, state_at_capture, Z](const State &,
- const Input &) -> Eigen::Vector3f {
- Eigen::Vector3f error = H * state_at_capture.value() - Z;
- error(2) = aos::math::NormalizeAngle(error(2));
- return error;
- },
- [H](const State &) { return H; }, R, now);
+ observations_.CorrectKnownH(Eigen::Vector3f::Zero(), &U_correct,
+ Corrector(state_at_capture.value(), Z), R, now);
}
}
diff --git a/y2022/control_loops/drivetrain/localizer.h b/y2022/control_loops/drivetrain/localizer.h
index 0d2673b..77b29eb 100644
--- a/y2022/control_loops/drivetrain/localizer.h
+++ b/y2022/control_loops/drivetrain/localizer.h
@@ -4,11 +4,11 @@
#include <string_view>
#include "aos/events/event_loop.h"
+#include "aos/network/message_bridge_server_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/drivetrain/localizer.h"
-#include "y2022/localizer/localizer_output_generated.h"
-#include "aos/network/message_bridge_server_generated.h"
#include "frc971/input/joystick_state_generated.h"
+#include "y2022/localizer/localizer_output_generated.h"
namespace y2022 {
namespace control_loops {
@@ -63,9 +63,34 @@
}
private:
+ class Corrector : public HybridEkf::ExpectedObservationFunctor {
+ public:
+ Corrector(const State &state_at_capture, const Eigen::Vector3f &Z)
+ : state_at_capture_(state_at_capture), Z_(Z) {
+ H_.setZero();
+ H_(0, StateIdx::kX) = 1;
+ H_(1, StateIdx::kY) = 1;
+ H_(2, StateIdx::kTheta) = 1;
+ }
+ Output H(const State &, const Input &) final {
+ Eigen::Vector3f error = H_ * state_at_capture_ - Z_;
+ error(2) = aos::math::NormalizeAngle(error(2));
+ return error;
+ }
+ Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> DHDX(
+ const State &) final {
+ return H_;
+ }
+
+ private:
+ Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> H_;
+ State state_at_capture_;
+ Eigen::Vector3f Z_;
+ };
aos::EventLoop *const event_loop_;
const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
HybridEkf ekf_;
+ HybridEkf::ExpectedObservationAllocator<Corrector> observations_;
aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
diff --git a/y2022/control_loops/drivetrain/localizer_test.cc b/y2022/control_loops/drivetrain/localizer_test.cc
index 1e33826..77f3988 100644
--- a/y2022/control_loops/drivetrain/localizer_test.cc
+++ b/y2022/control_loops/drivetrain/localizer_test.cc
@@ -10,12 +10,13 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/team_number_test_environment.h"
-#include "y2022/localizer/localizer_output_generated.h"
#include "gtest/gtest.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
+#include "y2022/localizer/localizer_output_generated.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
+DECLARE_bool(die_on_malloc);
namespace y2022 {
namespace control_loops {
@@ -31,7 +32,7 @@
DrivetrainConfig<double> config = GetDrivetrainConfig();
return config;
}
-}
+} // namespace
namespace chrono = std::chrono;
using aos::monotonic_clock;
@@ -74,6 +75,7 @@
drivetrain_plant_(drivetrain_plant_event_loop_.get(),
drivetrain_plant_imu_event_loop_.get(), dt_config_,
std::chrono::microseconds(500)) {
+ FLAGS_die_on_malloc = true;
set_team_id(frc971::control_loops::testing::kTeamNumber);
set_battery_voltage(12.0);
@@ -95,6 +97,7 @@
output_builder.add_x(drivetrain_plant_.state()(0));
output_builder.add_y(drivetrain_plant_.state()(1));
output_builder.add_theta(drivetrain_plant_.state()(2));
+ builder.CheckOk(builder.Send(output_builder.Finish()));
})
->Setup(imu_test_event_loop_->monotonic_now(),
std::chrono::milliseconds(5));
diff --git a/y2022/localizer/BUILD b/y2022/localizer/BUILD
index ec717fc..2a0a9fe 100644
--- a/y2022/localizer/BUILD
+++ b/y2022/localizer/BUILD
@@ -168,35 +168,15 @@
],
)
-cc_library(
- name = "imu",
- srcs = [
- "imu.cc",
- ],
- hdrs = [
- "imu.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- "//aos/events:epoll",
- "//aos/events:shm_event_loop",
- "//aos/util:crc32",
- "//frc971/wpilib:imu_batch_fbs",
- "//frc971/wpilib:imu_fbs",
- "//y2022:constants",
- "@com_github_google_glog//:glog",
- "@com_google_absl//absl/types:span",
- ],
-)
-
cc_binary(
name = "imu_main",
srcs = ["imu_main.cc"],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- ":imu",
"//aos:init",
"//aos/events:shm_event_loop",
+ "//frc971/imu_reader:imu",
+ "//y2022:constants",
],
)
diff --git a/y2022/localizer/imu_main.cc b/y2022/localizer/imu_main.cc
index 5bdab41..03dfc58 100644
--- a/y2022/localizer/imu_main.cc
+++ b/y2022/localizer/imu_main.cc
@@ -1,6 +1,8 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
-#include "y2022/localizer/imu.h"
+#include "aos/realtime.h"
+#include "frc971/imu_reader/imu.h"
+#include "y2022/constants.h"
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
@@ -10,8 +12,14 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig(FLAGS_config);
+ PCHECK(system("sudo chmod 644 /dev/adis16505") == 0)
+ << ": Failed to set read permissions on IMU device.";
+
aos::ShmEventLoop event_loop(&config.message());
- y2022::localizer::Imu imu(&event_loop);
+ frc971::imu::Imu imu(&event_loop,
+ y2022::constants::Values::DrivetrainEncoderToMeters(1));
+
+ event_loop.SetRuntimeRealtimePriority(30);
event_loop.Run();
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index ceec638..3d49755 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -299,29 +299,6 @@
)
cc_binary(
- name = "target_mapping",
- srcs = [
- "target_mapping.cc",
- ],
- data = [
- "//y2022:aos_config",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2022:__subpackages__"],
- deps = [
- ":camera_reader_lib",
- "//aos:init",
- "//aos/events:simulated_event_loop",
- "//aos/events/logging:log_reader",
- "//frc971/control_loops:pose",
- "//frc971/vision:charuco_lib",
- "//frc971/vision:target_mapper",
- "//third_party:opencv",
- "//y2022/control_loops/superstructure:superstructure_status_fbs",
- ],
-)
-
-cc_binary(
name = "calibrate_extrinsics",
srcs = [
"calibrate_extrinsics.cc",
diff --git a/y2022/vision/calibrate_extrinsics.cc b/y2022/vision/calibrate_extrinsics.cc
index 05d3481..04b24ea 100644
--- a/y2022/vision/calibrate_extrinsics.cc
+++ b/y2022/vision/calibrate_extrinsics.cc
@@ -2,6 +2,7 @@
#include "Eigen/Geometry"
#include "absl/strings/str_format.h"
#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
#include "aos/init.h"
#include "aos/network/team_number.h"
#include "aos/time/time.h"
@@ -19,8 +20,10 @@
DEFINE_bool(plot, false, "Whether to plot the resulting data.");
DEFINE_bool(turret, true, "If true, the camera is on the turret");
DEFINE_string(target_type, "charuco",
- "Type of target: april_tag|aruco|charuco|charuco_diamond");
+ "Type of target: aruco|charuco|charuco_diamond");
DEFINE_string(image_channel, "/camera", "Channel to listen for images on");
+DEFINE_string(output_logs, "/tmp/calibration/",
+ "Output folder for visualization logs.");
namespace frc971 {
namespace vision {
@@ -33,11 +36,24 @@
void Main(int argc, char **argv) {
CalibrationData data;
+ std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(FLAGS_pi);
+ CHECK(pi_number);
+ const std::string pi_name = absl::StrCat("pi", *pi_number);
+ LOG(INFO) << "Pi " << *pi_number;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config = [argc, argv,
+ pi_name]() {
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+ return CalibrationFoxgloveVisualizer::AddVisualizationChannels(
+ reader.logged_configuration(),
+ aos::configuration::GetNode(reader.logged_configuration(), pi_name));
+ }();
{
// Now, accumulate all the data into the data object.
aos::logger::LogReader reader(
- aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)),
+ &config.message());
aos::SimulatedEventLoopFactory factory(reader.configuration());
reader.Register(&factory);
@@ -50,11 +66,8 @@
const aos::Node *const roborio_node =
aos::configuration::GetNode(factory.configuration(), "roborio");
- std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(FLAGS_pi);
- CHECK(pi_number);
- LOG(INFO) << "Pi " << *pi_number;
- const aos::Node *const pi_node = aos::configuration::GetNode(
- factory.configuration(), absl::StrCat("pi", *pi_number));
+ const aos::Node *const pi_node =
+ aos::configuration::GetNode(factory.configuration(), pi_name);
LOG(INFO) << "imu " << aos::FlatbufferToJson(imu_node);
LOG(INFO) << "roboRIO " << aos::FlatbufferToJson(roborio_node);
@@ -67,10 +80,13 @@
std::unique_ptr<aos::EventLoop> pi_event_loop =
factory.MakeEventLoop("calibration", pi_node);
+ std::unique_ptr<aos::EventLoop> logger_loop =
+ factory.MakeEventLoop("logger", pi_node);
+ aos::logger::Logger logger(logger_loop.get());
+ logger.StartLoggingOnRun(FLAGS_output_logs);
+
TargetType target_type = TargetType::kCharuco;
- if (FLAGS_target_type == "april_tag") {
- target_type = TargetType::kAprilTag;
- } else if (FLAGS_target_type == "aruco") {
+ if (FLAGS_target_type == "aruco") {
target_type = TargetType::kAruco;
} else if (FLAGS_target_type == "charuco") {
target_type = TargetType::kCharuco;
@@ -78,7 +94,7 @@
target_type = TargetType::kCharucoDiamond;
} else {
LOG(FATAL) << "Unknown target type: " << FLAGS_target_type
- << ", expected: april_tag|aruco|charuco|charuco_diamond";
+ << ", expected: aruco|charuco|charuco_diamond";
}
// Now, hook Calibration up to everything.
diff --git a/y2022/vision/camera_definition.py b/y2022/vision/camera_definition.py
index 2f8a127..9d43ee4 100644
--- a/y2022/vision/camera_definition.py
+++ b/y2022/vision/camera_definition.py
@@ -135,6 +135,7 @@
camera_yaw = -90.0 * np.pi / 180.0
T = np.array([-10.5 * 0.0254, -5.0 * 0.0254, 28.5 * 0.0254])
elif team_number == 7971:
+ is_turret = False
# Cameras are flipped upside down
camera_roll = 180.0 * np.pi / 180.0
if pi_number == "pi1":
diff --git a/y2022/vision/target_mapping.cc b/y2022/vision/target_mapping.cc
deleted file mode 100644
index 090ac60..0000000
--- a/y2022/vision/target_mapping.cc
+++ /dev/null
@@ -1,307 +0,0 @@
-#include "aos/events/logging/log_reader.h"
-#include "aos/events/simulated_event_loop.h"
-#include "aos/init.h"
-#include "frc971/control_loops/pose.h"
-#include "frc971/vision/charuco_lib.h"
-#include "frc971/vision/target_mapper.h"
-#include "opencv2/aruco.hpp"
-#include "opencv2/calib3d.hpp"
-#include "opencv2/core/eigen.hpp"
-#include "opencv2/features2d.hpp"
-#include "opencv2/highgui.hpp"
-#include "opencv2/highgui/highgui.hpp"
-#include "opencv2/imgproc.hpp"
-#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
-#include "y2022/vision/camera_reader.h"
-
-DEFINE_string(json_path, "target_map.json",
- "Specify path for json with initial pose guesses.");
-DEFINE_int32(team_number, 971,
- "Use the calibration for a node with this team number");
-
-DEFINE_bool(
- use_robot_position, false,
- "If true, use localizer output messages to get the robot position, and "
- "superstructure status messages to get the turret angle, at the "
- "times of target detections. Currently does not work reliably on the box "
- "of pis.");
-
-DECLARE_string(image_channel);
-
-namespace y2022 {
-namespace vision {
-using frc971::vision::DataAdapter;
-using frc971::vision::PoseUtils;
-using frc971::vision::TargetMapper;
-namespace superstructure = ::y2022::control_loops::superstructure;
-
-// Find transformation from camera to robot reference frame
-Eigen::Affine3d CameraTransform(Eigen::Affine3d fixed_extrinsics,
- Eigen::Affine3d turret_extrinsics,
- double turret_position) {
- // Calculate the pose of the camera relative to the robot origin.
- Eigen::Affine3d H_robot_camrob =
- fixed_extrinsics *
- Eigen::Affine3d(frc971::control_loops::TransformationMatrixForYaw<double>(
- turret_position)) *
- turret_extrinsics;
-
- return H_robot_camrob;
-}
-
-// Change reference frame from camera to robot
-Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camrob_target,
- Eigen::Affine3d fixed_extrinsics,
- Eigen::Affine3d turret_extrinsics,
- double turret_position) {
- const Eigen::Affine3d H_robot_camrob =
- CameraTransform(fixed_extrinsics, turret_extrinsics, turret_position);
- const Eigen::Affine3d H_robot_target = H_robot_camrob * H_camrob_target;
- return H_robot_target;
-}
-
-// Add detected apriltag poses relative to the robot to
-// timestamped_target_detections
-void HandleAprilTag(aos::distributed_clock::time_point pi_distributed_time,
- std::vector<cv::Vec4i> april_ids,
- std::vector<Eigen::Vector3d> rvecs_eigen,
- std::vector<Eigen::Vector3d> tvecs_eigen,
- std::vector<DataAdapter::TimestampedDetection>
- *timestamped_target_detections,
- std::optional<aos::Fetcher<superstructure::Status>>
- *superstructure_status_fetcher,
- Eigen::Affine3d fixed_extrinsics,
- Eigen::Affine3d turret_extrinsics) {
- double turret_position = 0.0;
-
- if (superstructure_status_fetcher->has_value()) {
- CHECK(superstructure_status_fetcher->value().Fetch());
- turret_position =
- superstructure_status_fetcher->value().get()->turret()->position();
- }
-
- for (size_t tag = 0; tag < april_ids.size(); tag++) {
- Eigen::Translation3d T_camera_target = Eigen::Translation3d(
- tvecs_eigen[tag][0], tvecs_eigen[tag][1], tvecs_eigen[tag][2]);
- Eigen::AngleAxisd r_angle = Eigen::AngleAxisd(
- rvecs_eigen[tag].norm(), rvecs_eigen[tag] / rvecs_eigen[tag].norm());
- CHECK(rvecs_eigen[tag].norm() != 0) << "rvecs norm = 0; divide by 0";
-
- Eigen::Affine3d H_camcv_target = T_camera_target * r_angle;
- // With X, Y, Z being robot axes and x, y, z being camera axes,
- // x = -Y, y = -Z, z = X
- static const Eigen::Affine3d H_camcv_camrob =
- Eigen::Affine3d((Eigen::Matrix4d() << 0.0, -1.0, 0.0, 0.0, 0.0, 0.0,
- -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)
- .finished());
- Eigen::Affine3d H_camrob_target = H_camcv_camrob.inverse() * H_camcv_target;
- Eigen::Affine3d H_robot_target = CameraToRobotDetection(
- H_camrob_target, fixed_extrinsics, turret_extrinsics, turret_position);
-
- ceres::examples::Pose2d target_pose_camera =
- PoseUtils::Affine3dToPose2d(H_camrob_target);
- double distance_from_camera = std::sqrt(std::pow(target_pose_camera.x, 2) +
- std::pow(target_pose_camera.y, 2));
-
- timestamped_target_detections->emplace_back(
- DataAdapter::TimestampedDetection{
- .time = pi_distributed_time,
- .H_robot_target = H_robot_target,
- .distance_from_camera = distance_from_camera,
- .id = april_ids[tag][0]});
- }
-}
-
-Eigen::Affine3d CameraFixedExtrinsics(
- const calibration::CameraCalibration *camera_calibration) {
- cv::Mat extrinsics(
- 4, 4, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration->fixed_extrinsics()->data()->data())));
- extrinsics.convertTo(extrinsics, CV_64F);
- CHECK_EQ(extrinsics.total(),
- camera_calibration->fixed_extrinsics()->data()->size());
- Eigen::Matrix4d extrinsics_eigen;
- cv::cv2eigen(extrinsics, extrinsics_eigen);
- return Eigen::Affine3d(extrinsics_eigen);
-}
-
-Eigen::Affine3d CameraTurretExtrinsics(
- const calibration::CameraCalibration *camera_calibration) {
- cv::Mat extrinsics(
- 4, 4, CV_32F,
- const_cast<void *>(static_cast<const void *>(
- camera_calibration->turret_extrinsics()->data()->data())));
- extrinsics.convertTo(extrinsics, CV_64F);
- CHECK_EQ(extrinsics.total(),
- camera_calibration->turret_extrinsics()->data()->size());
- Eigen::Matrix4d extrinsics_eigen;
- cv::cv2eigen(extrinsics, extrinsics_eigen);
- return Eigen::Affine3d(extrinsics_eigen);
-}
-
-// Get images from pi and pass apriltag positions to HandleAprilTag()
-void HandlePiCaptures(
- int pi_number, aos::EventLoop *pi_event_loop,
- aos::logger::LogReader *reader,
- std::optional<aos::Fetcher<superstructure::Status>>
- *superstructure_status_fetcher,
- std::vector<DataAdapter::TimestampedDetection>
- *timestamped_target_detections,
- std::vector<std::unique_ptr<CharucoExtractor>> *charuco_extractors,
- std::vector<std::unique_ptr<ImageCallback>> *image_callbacks) {
- const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
- CalibrationData());
- const calibration::CameraCalibration *calibration =
- CameraReader::FindCameraCalibration(&calibration_data.message(),
- "pi" + std::to_string(pi_number),
- FLAGS_team_number);
- const auto turret_extrinsics = CameraTurretExtrinsics(calibration);
- const auto fixed_extrinsics = CameraFixedExtrinsics(calibration);
-
- // TODO(milind): change to /camera once we log at full frequency
- static constexpr std::string_view kImageChannel = "/camera/decimated";
- charuco_extractors->emplace_back(std::make_unique<CharucoExtractor>(
- pi_event_loop,
- "pi-" + std::to_string(FLAGS_team_number) + "-" +
- std::to_string(pi_number),
- TargetType::kAprilTag, kImageChannel,
- [=](cv::Mat /*rgb_image*/, aos::monotonic_clock::time_point eof,
- std::vector<cv::Vec4i> april_ids,
- std::vector<std::vector<cv::Point2f>> /*april_corners*/, bool valid,
- std::vector<Eigen::Vector3d> rvecs_eigen,
- std::vector<Eigen::Vector3d> tvecs_eigen) {
- aos::distributed_clock::time_point pi_distributed_time =
- reader->event_loop_factory()
- ->GetNodeEventLoopFactory(pi_event_loop->node())
- ->ToDistributedClock(eof);
-
- if (valid) {
- HandleAprilTag(pi_distributed_time, april_ids, rvecs_eigen,
- tvecs_eigen, timestamped_target_detections,
- superstructure_status_fetcher, fixed_extrinsics,
- turret_extrinsics);
- }
- }));
-
- image_callbacks->emplace_back(std::make_unique<ImageCallback>(
- pi_event_loop, kImageChannel,
- [&, charuco_extractor =
- charuco_extractors->at(charuco_extractors->size() - 1).get()](
- cv::Mat rgb_image, const aos::monotonic_clock::time_point eof) {
- charuco_extractor->HandleImage(rgb_image, eof);
- }));
-}
-
-void MappingMain(int argc, char *argv[]) {
- std::vector<std::string> unsorted_logfiles =
- aos::logger::FindLogs(argc, argv);
-
- std::vector<DataAdapter::TimestampedPose> timestamped_robot_poses;
- std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections;
-
- // open logfiles
- aos::logger::LogReader reader(aos::logger::SortParts(unsorted_logfiles));
- reader.Register();
-
- std::optional<aos::Fetcher<superstructure::Status>>
- superstructure_status_fetcher;
-
- if (FLAGS_use_robot_position) {
- const aos::Node *imu_node =
- aos::configuration::GetNode(reader.configuration(), "imu");
- std::unique_ptr<aos::EventLoop> imu_event_loop =
- reader.event_loop_factory()->MakeEventLoop("imu", imu_node);
-
- imu_event_loop->MakeWatcher(
- "/localizer", [&](const frc971::controls::LocalizerOutput &localizer) {
- aos::monotonic_clock::time_point imu_monotonic_time =
- aos::monotonic_clock::time_point(aos::monotonic_clock::duration(
- localizer.monotonic_timestamp_ns()));
- aos::distributed_clock::time_point imu_distributed_time =
- reader.event_loop_factory()
- ->GetNodeEventLoopFactory(imu_node)
- ->ToDistributedClock(imu_monotonic_time);
-
- timestamped_robot_poses.emplace_back(DataAdapter::TimestampedPose{
- .time = imu_distributed_time,
- .pose =
- ceres::examples::Pose2d{.x = localizer.x(),
- .y = localizer.y(),
- .yaw_radians = localizer.theta()}});
- });
-
- const aos::Node *roborio_node =
- aos::configuration::GetNode(reader.configuration(), "roborio");
- std::unique_ptr<aos::EventLoop> roborio_event_loop =
- reader.event_loop_factory()->MakeEventLoop("roborio", roborio_node);
-
- superstructure_status_fetcher =
- roborio_event_loop->MakeFetcher<superstructure::Status>(
- "/superstructure");
- }
-
- std::vector<std::unique_ptr<CharucoExtractor>> charuco_extractors;
- std::vector<std::unique_ptr<ImageCallback>> image_callbacks;
-
- const aos::Node *pi1 =
- aos::configuration::GetNode(reader.configuration(), "pi1");
- std::unique_ptr<aos::EventLoop> pi1_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi1", pi1);
- HandlePiCaptures(
- 1, pi1_event_loop.get(), &reader, &superstructure_status_fetcher,
- ×tamped_target_detections, &charuco_extractors, &image_callbacks);
-
- const aos::Node *pi2 =
- aos::configuration::GetNode(reader.configuration(), "pi2");
- std::unique_ptr<aos::EventLoop> pi2_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi2", pi2);
- HandlePiCaptures(
- 2, pi2_event_loop.get(), &reader, &superstructure_status_fetcher,
- ×tamped_target_detections, &charuco_extractors, &image_callbacks);
-
- const aos::Node *pi3 =
- aos::configuration::GetNode(reader.configuration(), "pi3");
- std::unique_ptr<aos::EventLoop> pi3_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi3", pi3);
- HandlePiCaptures(
- 3, pi3_event_loop.get(), &reader, &superstructure_status_fetcher,
- ×tamped_target_detections, &charuco_extractors, &image_callbacks);
-
- const aos::Node *pi4 =
- aos::configuration::GetNode(reader.configuration(), "pi4");
- std::unique_ptr<aos::EventLoop> pi4_event_loop =
- reader.event_loop_factory()->MakeEventLoop("pi4", pi4);
- HandlePiCaptures(
- 4, pi4_event_loop.get(), &reader, &superstructure_status_fetcher,
- ×tamped_target_detections, &charuco_extractors, &image_callbacks);
-
- reader.event_loop_factory()->Run();
-
- auto target_constraints =
- (FLAGS_use_robot_position
- ? DataAdapter::MatchTargetDetections(timestamped_robot_poses,
- timestamped_target_detections)
- .first
- : DataAdapter::MatchTargetDetections(timestamped_target_detections));
-
- frc971::vision::TargetMapper mapper(FLAGS_json_path, target_constraints);
- mapper.Solve("rapid_react");
-
- // Pointers need to be deleted to destruct all fetchers
- for (auto &charuco_extractor_ptr : charuco_extractors) {
- charuco_extractor_ptr.reset();
- }
-
- for (auto &image_callback_ptr : image_callbacks) {
- image_callback_ptr.reset();
- }
-}
-
-} // namespace vision
-} // namespace y2022
-
-int main(int argc, char **argv) {
- aos::InitGoogle(&argc, &argv);
- y2022::vision::MappingMain(argc, argv);
-}
diff --git a/y2022/y2022_imu.json b/y2022/y2022_imu.json
index bd2b326..bbe3a11 100644
--- a/y2022/y2022_imu.json
+++ b/y2022/y2022_imu.json
@@ -229,7 +229,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "logger",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
@@ -287,7 +287,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "roborio",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
@@ -351,7 +351,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "imu",
"logger": "NOT_LOGGED",
- "frequency": 200,
+ "frequency": 400,
"num_senders": 2,
"max_size": 200
},
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index 0f790c7..01800bf 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -237,7 +237,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "logger",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
diff --git a/y2022/y2022_pi_template.json b/y2022/y2022_pi_template.json
index 99b04a1..4d3c427 100644
--- a/y2022/y2022_pi_template.json
+++ b/y2022/y2022_pi_template.json
@@ -218,14 +218,14 @@
{
"name": "/pi{{ NUM }}/aos/remote_timestamps/imu/pi{{ NUM }}/camera/y2022-vision-TargetEstimate",
"type": "aos.message_bridge.RemoteMessage",
- "frequency": 50,
+ "frequency": 80,
"source_node": "pi{{ NUM }}",
"max_size": 208
},
{
"name": "/pi{{ NUM }}/aos/remote_timestamps/logger/pi{{ NUM }}/camera/y2022-vision-TargetEstimate",
"type": "aos.message_bridge.RemoteMessage",
- "frequency": 50,
+ "frequency": 80,
"source_node": "pi{{ NUM }}",
"max_size": 208
},
@@ -300,7 +300,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "logger",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
@@ -350,7 +350,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "roborio",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
}
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index 85247ee..ff8d0c6 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -99,7 +99,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "roborio",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
diff --git a/y2022_bot3/y2022_bot3_imu.json b/y2022_bot3/y2022_bot3_imu.json
index 5673195..195592e 100644
--- a/y2022_bot3/y2022_bot3_imu.json
+++ b/y2022_bot3/y2022_bot3_imu.json
@@ -145,7 +145,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "roborio",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
diff --git a/y2022_bot3/y2022_bot3_roborio.json b/y2022_bot3/y2022_bot3_roborio.json
index 5f0b5ae..6485ed8 100644
--- a/y2022_bot3/y2022_bot3_roborio.json
+++ b/y2022_bot3/y2022_bot3_roborio.json
@@ -82,14 +82,14 @@
],
"time_to_live": 5000000
}
- ]
+ ]
},
{
"name": "/roborio/aos/remote_timestamps/logger/roborio/aos/aos-starter-Status",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "roborio",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
diff --git a/y2023/BUILD b/y2023/BUILD
index dbc069a..0c34ce8 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -7,7 +7,7 @@
binaries = [
"//y2020/vision:calibration",
"//y2023/vision:viewer",
- "//y2022/localizer:imu_main",
+ "//y2023/vision:aprilrobotics",
"//y2022/localizer:localizer_main",
"//aos/network:web_proxy_main",
"//aos/events/logging:log_cat",
@@ -16,8 +16,6 @@
":aos_config",
":message_bridge_client.sh",
"//y2022/www:www_files",
- "@ctre_phoenix_api_cpp_athena//:shared_libraries",
- "@ctre_phoenix_cci_athena//:shared_libraries",
],
dirs = [
"//y2023/www:www_files",
@@ -29,12 +27,6 @@
"//aos/starter:irq_affinity",
"//y2023/vision:camera_reader",
"//aos/events/logging:logger_main",
- ":joystick_reader",
- ":wpilib_interface",
- "//y2023/autonomous:binaries",
- "//y2023/control_loops/drivetrain:drivetrain",
- "//y2023/control_loops/drivetrain:trajectory_generator",
- "//y2023/control_loops/superstructure:superstructure",
],
target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
target_type = "pi",
@@ -49,6 +41,7 @@
"//aos/network:timestamp_fbs",
"//frc971/input:robot_state_fbs",
"//frc971/vision:vision_fbs",
+ "//frc971/vision:target_map_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -74,6 +67,7 @@
"//aos/network:remote_message_fbs",
"//y2022/localizer:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
+ "//frc971/vision:target_map_fbs",
"//frc971/vision:vision_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
@@ -103,6 +97,7 @@
"//y2022/localizer:localizer_status_fbs",
"//y2022/localizer:localizer_output_fbs",
"//y2022/localizer:localizer_visualization_fbs",
+ "//frc971/vision:target_map_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -122,6 +117,7 @@
"//aos/network:remote_message_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:vision_fbs",
+ "//frc971/vision:target_map_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
diff --git a/y2023/control_loops/python/BUILD b/y2023/control_loops/python/BUILD
index b024676..9b1746a 100644
--- a/y2023/control_loops/python/BUILD
+++ b/y2023/control_loops/python/BUILD
@@ -31,6 +31,25 @@
],
)
+py_binary(
+ name = "graph_edit",
+ srcs = [
+ "graph_edit.py",
+ "graph_paths.py",
+ "graph_tools.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//frc971/control_loops/python:basic_window",
+ "//frc971/control_loops/python:color",
+ "@pip//numpy",
+ "@pip//pygobject",
+ "@pip//shapely",
+ ],
+)
+
py_library(
name = "polydrivetrain_lib",
srcs = [
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
new file mode 100644
index 0000000..39cd814
--- /dev/null
+++ b/y2023/control_loops/python/graph_edit.py
@@ -0,0 +1,475 @@
+#!/usr/bin/python3
+
+from __future__ import print_function
+import os
+from frc971.control_loops.python import basic_window
+from frc971.control_loops.python.color import Color, palette
+import random
+import gi
+import numpy
+
+gi.require_version('Gtk', '3.0')
+from gi.repository import Gdk, Gtk
+import cairo
+from graph_tools import XYSegment, AngleSegment, to_theta, to_xy, alpha_blend, draw_lines
+from graph_tools import back_to_xy_loop, subdivide_theta, to_theta_loop, px
+from graph_tools import l1, l2, joint_center
+import graph_paths
+
+from frc971.control_loops.python.basic_window import quit_main_loop, set_color
+
+import shapely
+from shapely.geometry import Polygon
+
+
+def draw_px_cross(cr, length_px):
+ """Draws a cross with fixed dimensions in pixel space."""
+ with px(cr):
+ x, y = cr.get_current_point()
+ cr.move_to(x, y - length_px)
+ cr.line_to(x, y + length_px)
+ cr.stroke()
+
+ cr.move_to(x - length_px, y)
+ cr.line_to(x + length_px, y)
+ cr.stroke()
+
+
+def angle_dist_sqr(a1, a2):
+ """Distance between two points in angle space."""
+ return (a1[0] - a2[0])**2 + (a1[1] - a2[1])**2
+
+
+# Find the highest y position that intersects the vertical line defined by x.
+def inter_y(x):
+ return numpy.sqrt((l2 + l1)**2 -
+ (x - joint_center[0])**2) + joint_center[1]
+
+
+# This is the x position where the inner (hyperextension) circle intersects the horizontal line
+derr = numpy.sqrt((l1 - l2)**2 - (joint_center[1] - 0.3048)**2)
+
+
+# Define min and max l1 angles based on vertical constraints.
+def get_angle(boundary):
+ h = numpy.sqrt((l1)**2 - (boundary - joint_center[0])**2) + joint_center[1]
+ return numpy.arctan2(h, boundary - joint_center[0])
+
+
+# left hand side lines
+lines1 = [
+ (-0.826135, inter_y(-0.826135)),
+ (-0.826135, 0.1397),
+ (-23.025 * 0.0254, 0.1397),
+ (-23.025 * 0.0254, 0.3048),
+ (joint_center[0] - derr, 0.3048),
+]
+
+# right hand side lines
+lines2 = [(joint_center[0] + derr, 0.3048), (0.422275, 0.3048),
+ (0.422275, 0.1397), (0.826135, 0.1397),
+ (0.826135, inter_y(0.826135))]
+
+t1_min = get_angle((32.525 - 4.0) * 0.0254)
+t2_min = -7.0 / 4.0 * numpy.pi
+
+t1_max = get_angle((-32.525 + 4.0) * 0.0254)
+t2_max = numpy.pi * 3.0 / 4.0
+
+
+# Rotate a rasterized loop such that it aligns to when the parameters loop
+def rotate_to_jump_point(points):
+ last_pt = points[0]
+ for pt_i in range(1, len(points)):
+ pt = points[pt_i]
+ delta = last_pt[1] - pt[1]
+ if abs(delta) > numpy.pi:
+ return points[pt_i:] + points[:pt_i]
+ last_pt = pt
+ return points
+
+
+# shift points vertically by dy.
+def y_shift(points, dy):
+ return [(x, y + dy) for x, y in points]
+
+
+lines1_theta_part = rotate_to_jump_point(to_theta_loop(lines1, 0))
+lines2_theta_part = rotate_to_jump_point(to_theta_loop(lines2))
+
+# Some hacks here to make a single polygon by shifting to get an extra copy of the contraints.
+lines1_theta = y_shift(lines1_theta_part, -numpy.pi * 2) + lines1_theta_part + \
+ y_shift(lines1_theta_part, numpy.pi * 2)
+lines2_theta = y_shift(lines2_theta_part, numpy.pi * 2) + lines2_theta_part + \
+ y_shift(lines2_theta_part, -numpy.pi * 2)
+
+lines_theta = lines1_theta + lines2_theta
+
+p1 = Polygon(lines_theta)
+
+p2 = Polygon([(t1_min, t2_min), (t1_max, t2_min), (t1_max, t2_max),
+ (t1_min, t2_max)])
+
+# Fully computed theta constrints.
+lines_theta = list(p1.intersection(p2).exterior.coords)
+
+lines1_theta_back = back_to_xy_loop(lines1_theta)
+lines2_theta_back = back_to_xy_loop(lines2_theta)
+
+lines_theta_back = back_to_xy_loop(lines_theta)
+
+
+# Get the closest point to a line from a test pt.
+def get_closest(prev, cur, pt):
+ dx_ang = (cur[0] - prev[0])
+ dy_ang = (cur[1] - prev[1])
+
+ d = numpy.sqrt(dx_ang**2 + dy_ang**2)
+ if (d < 0.000001):
+ return prev, numpy.sqrt((prev[0] - pt[0])**2 + (prev[1] - pt[1])**2)
+
+ pdx = -dy_ang / d
+ pdy = dx_ang / d
+
+ dpx = pt[0] - prev[0]
+ dpy = pt[1] - prev[1]
+
+ alpha = (dx_ang * dpx + dy_ang * dpy) / d / d
+
+ if (alpha < 0):
+ return prev, numpy.sqrt((prev[0] - pt[0])**2 + (prev[1] - pt[1])**2)
+ elif (alpha > 1):
+ return cur, numpy.sqrt((cur[0] - pt[0])**2 + (cur[1] - pt[1])**2)
+ else:
+ return (alpha_blend(prev[0], cur[0], alpha), alpha_blend(prev[1], cur[1], alpha)), \
+ abs(dpx * pdx + dpy * pdy)
+
+
+def closest_segment(lines, pt):
+ c_pt, c_pt_dist = get_closest(lines[-1], lines[0], pt)
+ for i in range(1, len(lines)):
+ prev = lines[i - 1]
+ cur = lines[i]
+ c_pt_new, c_pt_new_dist = get_closest(prev, cur, pt)
+ if c_pt_new_dist < c_pt_dist:
+ c_pt = c_pt_new
+ c_pt_dist = c_pt_new_dist
+ return c_pt, c_pt_dist
+
+
+# Create a GTK+ widget on which we will draw using Cairo
+class Silly(basic_window.BaseWindow):
+
+ def __init__(self):
+ super(Silly, self).__init__()
+
+ self.window = Gtk.Window()
+ self.window.set_title("DrawingArea")
+
+ self.window.set_events(Gdk.EventMask.BUTTON_PRESS_MASK
+ | Gdk.EventMask.BUTTON_RELEASE_MASK
+ | Gdk.EventMask.POINTER_MOTION_MASK
+ | Gdk.EventMask.SCROLL_MASK
+ | Gdk.EventMask.KEY_PRESS_MASK)
+ self.method_connect("key-press-event", self.do_key_press)
+ self.method_connect("button-press-event",
+ self._do_button_press_internal)
+ self.method_connect("configure-event", self._do_configure)
+ self.window.add(self)
+ self.window.show_all()
+
+ self.theta_version = False
+ self.reinit_extents()
+
+ self.last_pos = (numpy.pi / 2.0, 1.0)
+ self.circular_index_select = -1
+
+ # Extra stuff for drawing lines.
+ self.segments = []
+ self.prev_segment_pt = None
+ self.now_segment_pt = None
+ self.spline_edit = 0
+ self.edit_control1 = True
+
+ def do_key_press(self, event):
+ pass
+
+ def _do_button_press_internal(self, event):
+ o_x = event.x
+ o_y = event.y
+ x = event.x - self.window_shape[0] / 2
+ y = self.window_shape[1] / 2 - event.y
+ scale = self.get_current_scale()
+ event.x = x / scale + self.center[0]
+ event.y = y / scale + self.center[1]
+ self.do_button_press(event)
+ event.x = o_x
+ event.y = o_y
+
+ def _do_configure(self, event):
+ self.window_shape = (event.width, event.height)
+
+ def redraw(self):
+ if not self.needs_redraw:
+ self.needs_redraw = True
+ self.window.queue_draw()
+
+ def method_connect(self, event, cb):
+
+ def handler(obj, *args):
+ cb(*args)
+
+ self.window.connect(event, handler)
+
+ def reinit_extents(self):
+ if self.theta_version:
+ self.extents_x_min = -numpy.pi * 2
+ self.extents_x_max = numpy.pi * 2
+ self.extents_y_min = -numpy.pi * 2
+ self.extents_y_max = numpy.pi * 2
+ else:
+ self.extents_x_min = -40.0 * 0.0254
+ self.extents_x_max = 40.0 * 0.0254
+ self.extents_y_min = -4.0 * 0.0254
+ self.extents_y_max = 110.0 * 0.0254
+
+ self.init_extents(
+ (0.5 * (self.extents_x_min + self.extents_x_max), 0.5 *
+ (self.extents_y_max + self.extents_y_min)),
+ (1.0 * (self.extents_x_max - self.extents_x_min), 1.0 *
+ (self.extents_y_max - self.extents_y_min)))
+
+ # Handle the expose-event by drawing
+ def handle_draw(self, cr):
+ # use "with px(cr): blah;" to transform to pixel coordinates.
+
+ # Fill the background color of the window with grey
+ set_color(cr, palette["GREY"])
+ cr.paint()
+
+ # Draw a extents rectangle
+ set_color(cr, palette["WHITE"])
+ cr.rectangle(self.extents_x_min, self.extents_y_min,
+ (self.extents_x_max - self.extents_x_min),
+ self.extents_y_max - self.extents_y_min)
+ cr.fill()
+
+ if self.theta_version:
+ # Draw a filled white rectangle.
+ set_color(cr, palette["WHITE"])
+ cr.rectangle(-numpy.pi, -numpy.pi, numpy.pi * 2.0, numpy.pi * 2.0)
+ cr.fill()
+
+ set_color(cr, palette["BLUE"])
+ for i in range(-6, 6):
+ cr.move_to(-40, -40 + i * numpy.pi)
+ cr.line_to(40, 40 + i * numpy.pi)
+ with px(cr):
+ cr.stroke()
+
+ set_color(cr, Color(0.5, 0.5, 1.0))
+ draw_lines(cr, lines_theta)
+
+ set_color(cr, Color(0.0, 1.0, 0.2))
+ cr.move_to(self.last_pos[0], self.last_pos[1])
+ draw_px_cross(cr, 5)
+
+ c_pt, dist = closest_segment(lines_theta, self.last_pos)
+ print("dist:", dist, c_pt, self.last_pos)
+ set_color(cr, palette["CYAN"])
+ cr.move_to(c_pt[0], c_pt[1])
+ draw_px_cross(cr, 5)
+ else:
+ # Draw a filled white rectangle.
+ set_color(cr, palette["WHITE"])
+ cr.rectangle(-2.0, -2.0, 4.0, 4.0)
+ cr.fill()
+
+ set_color(cr, palette["BLUE"])
+ cr.arc(joint_center[0], joint_center[1], l2 + l1, 0,
+ 2.0 * numpy.pi)
+ with px(cr):
+ cr.stroke()
+ cr.arc(joint_center[0], joint_center[1], l1 - l2, 0,
+ 2.0 * numpy.pi)
+ with px(cr):
+ cr.stroke()
+
+ set_color(cr, Color(0.5, 1.0, 1.0))
+ draw_lines(cr, lines1)
+ draw_lines(cr, lines2)
+
+ def get_circular_index(pt):
+ theta1, theta2 = pt
+ circular_index = int(numpy.floor((theta2 - theta1) / numpy.pi))
+ return circular_index
+
+ set_color(cr, palette["BLUE"])
+ lines = subdivide_theta(lines_theta)
+ o_circular_index = circular_index = get_circular_index(lines[0])
+ p_xy = to_xy(lines[0][0], lines[0][1])
+ if circular_index == self.circular_index_select:
+ cr.move_to(p_xy[0] + circular_index * 0, p_xy[1])
+ for pt in lines[1:]:
+ p_xy = to_xy(pt[0], pt[1])
+ circular_index = get_circular_index(pt)
+ if o_circular_index == self.circular_index_select:
+ cr.line_to(p_xy[0] + o_circular_index * 0, p_xy[1])
+ if circular_index != o_circular_index:
+ o_circular_index = circular_index
+ with px(cr):
+ cr.stroke()
+ if circular_index == self.circular_index_select:
+ cr.move_to(p_xy[0] + circular_index * 0, p_xy[1])
+
+ with px(cr):
+ cr.stroke()
+
+ theta1, theta2 = to_theta(self.last_pos,
+ self.circular_index_select)
+ x, y = joint_center[0], joint_center[1]
+ cr.move_to(x, y)
+
+ x += numpy.cos(theta1) * l1
+ y += numpy.sin(theta1) * l1
+ cr.line_to(x, y)
+ x += numpy.cos(theta2) * l2
+ y += numpy.sin(theta2) * l2
+ cr.line_to(x, y)
+ with px(cr):
+ cr.stroke()
+
+ cr.move_to(self.last_pos[0], self.last_pos[1])
+ set_color(cr, Color(0.0, 1.0, 0.2))
+ draw_px_cross(cr, 20)
+
+ set_color(cr, Color(0.0, 0.5, 1.0))
+ for segment in self.segments:
+ color = [0, random.random(), 1]
+ random.shuffle(color)
+ set_color(cr, Color(color[0], color[1], color[2]))
+ segment.DrawTo(cr, self.theta_version)
+ with px(cr):
+ cr.stroke()
+
+ set_color(cr, Color(0.0, 1.0, 0.5))
+ segment = self.current_seg()
+ if segment:
+ print(segment)
+ segment.DrawTo(cr, self.theta_version)
+ with px(cr):
+ cr.stroke()
+
+ def cur_pt_in_theta(self):
+ if self.theta_version: return self.last_pos
+ return to_theta(self.last_pos, self.circular_index_select)
+
+ # Current segment based on which mode the drawing system is in.
+ def current_seg(self):
+ if self.prev_segment_pt and self.now_segment_pt:
+ if self.theta_version:
+ return AngleSegment(self.prev_segment_pt, self.now_segment_pt)
+ else:
+ return XYSegment(self.prev_segment_pt, self.now_segment_pt)
+
+ def do_key_press(self, event):
+ keyval = Gdk.keyval_to_lower(event.keyval)
+ print("Gdk.KEY_" + Gdk.keyval_name(keyval))
+ if keyval == Gdk.KEY_q:
+ print("Found q key and exiting.")
+ quit_main_loop()
+ elif keyval == Gdk.KEY_c:
+ # Increment which arm solution we render
+ self.circular_index_select += 1
+ print(self.circular_index_select)
+ elif keyval == Gdk.KEY_v:
+ # Decrement which arm solution we render
+ self.circular_index_select -= 1
+ print(self.circular_index_select)
+ elif keyval == Gdk.KEY_w:
+ # Add this segment to the segment list.
+ segment = self.current_seg()
+ if segment: self.segments.append(segment)
+ self.prev_segment_pt = self.now_segment_pt
+
+ elif keyval == Gdk.KEY_r:
+ self.prev_segment_pt = self.now_segment_pt
+
+ elif keyval == Gdk.KEY_p:
+ # Print out the segments.
+ print(repr(self.segments))
+ elif keyval == Gdk.KEY_g:
+ # Generate theta points.
+ if self.segments:
+ print(repr(self.segments[0].ToThetaPoints()))
+ elif keyval == Gdk.KEY_e:
+ best_pt = self.now_segment_pt
+ best_dist = 1e10
+ for segment in self.segments:
+ d = angle_dist_sqr(segment.start, self.now_segment_pt)
+ if (d < best_dist):
+ best_pt = segment.start
+ best_dist = d
+ d = angle_dist_sqr(segment.end, self.now_segment_pt)
+ if (d < best_dist):
+ best_pt = segment.end
+ best_dist = d
+ self.now_segment_pt = best_pt
+
+ elif keyval == Gdk.KEY_t:
+ # Toggle between theta and xy renderings
+ if self.theta_version:
+ theta1, theta2 = self.last_pos
+ data = to_xy(theta1, theta2)
+ self.circular_index_select = int(
+ numpy.floor((theta2 - theta1) / numpy.pi))
+ self.last_pos = (data[0], data[1])
+ else:
+ self.last_pos = self.cur_pt_in_theta()
+
+ self.theta_version = not self.theta_version
+ self.reinit_extents()
+
+ elif keyval == Gdk.KEY_z:
+ self.edit_control1 = not self.edit_control1
+ if self.edit_control1:
+ self.now_segment_pt = self.segments[0].control1
+ else:
+ self.now_segment_pt = self.segments[0].control2
+ if not self.theta_version:
+ data = to_xy(self.now_segment_pt[0], self.now_segment_pt[1])
+ self.last_pos = (data[0], data[1])
+ else:
+ self.last_pos = self.now_segment_pt
+
+ print("self.last_pos: ", self.last_pos, " ci: ",
+ self.circular_index_select)
+
+ self.redraw()
+
+ def do_button_press(self, event):
+ self.last_pos = (event.x, event.y)
+ self.now_segment_pt = self.cur_pt_in_theta()
+
+ if self.edit_control1:
+ self.segments[0].control1 = self.now_segment_pt
+ else:
+ self.segments[0].control2 = self.now_segment_pt
+
+ print('Clicked at theta: %s' % (repr(self.now_segment_pt, )))
+ if not self.theta_version:
+ print('Clicked at xy, circular index: (%f, %f, %f)' %
+ (self.last_pos[0], self.last_pos[1],
+ self.circular_index_select))
+
+ print('c1: numpy.array([%f, %f])' %
+ (self.segments[0].control1[0], self.segments[0].control1[1]))
+ print('c2: numpy.array([%f, %f])' %
+ (self.segments[0].control2[0], self.segments[0].control2[1]))
+
+ self.redraw()
+
+
+silly = Silly()
+silly.segments = graph_paths.segments
+basic_window.RunApp()
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
new file mode 100644
index 0000000..da0ad4f
--- /dev/null
+++ b/y2023/control_loops/python/graph_paths.py
@@ -0,0 +1,26 @@
+import numpy
+
+from graph_tools import *
+
+neutral = to_theta_with_circular_index(-0.2, 0.33, circular_index=-1)
+zero = to_theta_with_circular_index(0.0, 0.0, circular_index=-1)
+
+neutral_to_cone_1 = to_theta_with_circular_index(0.0, 0.7, circular_index=-1)
+neutral_to_cone_2 = to_theta_with_circular_index(0.2, 0.5, circular_index=-1)
+cone_pos = to_theta_with_circular_index(1.0, 0.4, circular_index=-1)
+
+neutral_to_cone_perch_pos_1 = to_theta_with_circular_index(0.4,
+ 1.0,
+ circular_index=-1)
+neutral_to_cone_perch_pos_2 = to_theta_with_circular_index(0.7,
+ 1.5,
+ circular_index=-1)
+cone_perch_pos = to_theta_with_circular_index(1.0, 2.0, circular_index=-1)
+
+segments = [
+ ThetaSplineSegment(neutral, neutral_to_cone_1, neutral_to_cone_2, cone_pos,
+ "NeutralToCone"),
+ ThetaSplineSegment(neutral, neutral_to_cone_perch_pos_1,
+ neutral_to_cone_perch_pos_2, cone_perch_pos,
+ "NeutralToPerchedCone"),
+]
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
new file mode 100644
index 0000000..3ddfb37
--- /dev/null
+++ b/y2023/control_loops/python/graph_tools.py
@@ -0,0 +1,533 @@
+import numpy
+import cairo
+
+from frc971.control_loops.python.basic_window import OverrideMatrix, identity
+
+# joint_center in x-y space.
+joint_center = (-0.299, 0.299)
+
+# Joint distances (l1 = "proximal", l2 = "distal")
+l1 = 46.25 * 0.0254
+l2 = 43.75 * 0.0254
+
+max_dist = 0.01
+max_dist_theta = numpy.pi / 64
+xy_end_circle_size = 0.01
+theta_end_circle_size = 0.07
+
+
+def px(cr):
+ return OverrideMatrix(cr, identity)
+
+
+# Convert from x-y coordinates to theta coordinates.
+# orientation is a bool. This orientation is circular_index mod 2.
+# where circular_index is the circular index, or the position in the
+# "hyperextension" zones. "cross_point" allows shifting the place where
+# it rounds the result so that it draws nicer (no other functional differences).
+def to_theta(pt, circular_index, cross_point=-numpy.pi):
+ orient = (circular_index % 2) == 0
+ x = pt[0]
+ y = pt[1]
+ x -= joint_center[0]
+ y -= joint_center[1]
+ l3 = numpy.hypot(x, y)
+ t3 = numpy.arctan2(y, x)
+ theta1 = numpy.arccos((l1**2 + l3**2 - l2**2) / (2 * l1 * l3))
+
+ if orient:
+ theta1 = -theta1
+ theta1 += t3
+ theta1 = (theta1 - cross_point) % (2 * numpy.pi) + cross_point
+ theta2 = numpy.arctan2(y - l1 * numpy.sin(theta1),
+ x - l1 * numpy.cos(theta1))
+ return numpy.array((theta1, theta2))
+
+
+# Simple trig to go back from theta1, theta2 to x-y
+def to_xy(theta1, theta2):
+ x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
+ y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
+ orient = ((theta2 - theta1) % (2.0 * numpy.pi)) < numpy.pi
+ return (x, y, orient)
+
+
+def get_circular_index(theta):
+ return int(numpy.floor((theta[1] - theta[0]) / numpy.pi))
+
+
+def get_xy(theta):
+ theta1 = theta[0]
+ theta2 = theta[1]
+ x = numpy.cos(theta1) * l1 + numpy.cos(theta2) * l2 + joint_center[0]
+ y = numpy.sin(theta1) * l1 + numpy.sin(theta2) * l2 + joint_center[1]
+ return numpy.array((x, y))
+
+
+# Subdivide in theta space.
+def subdivide_theta(lines):
+ out = []
+ last_pt = lines[0]
+ out.append(last_pt)
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist_theta):
+ out.append(pt)
+ last_pt = n_pt
+
+ return out
+
+
+# subdivide in xy space.
+def subdivide_xy(lines, max_dist=max_dist):
+ out = []
+ last_pt = lines[0]
+ out.append(last_pt)
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist):
+ out.append(pt)
+ last_pt = n_pt
+
+ return out
+
+
+def to_theta_with_ci(pt, circular_index):
+ return to_theta_with_circular_index(pt[0], pt[1], circular_index)
+
+
+# to_theta, but distinguishes between
+def to_theta_with_circular_index(x, y, circular_index):
+ theta1, theta2 = to_theta((x, y), circular_index)
+ n_circular_index = int(numpy.floor((theta2 - theta1) / numpy.pi))
+ theta2 = theta2 + ((circular_index - n_circular_index)) * numpy.pi
+ return numpy.array((theta1, theta2))
+
+
+# alpha is in [0, 1] and is the weight to merge a and b.
+def alpha_blend(a, b, alpha):
+ """Blends a and b.
+
+ Args:
+ alpha: double, Ratio. Needs to be in [0, 1] and is the weight to blend a
+ and b.
+ """
+ return b * alpha + (1.0 - alpha) * a
+
+
+def normalize(v):
+ """Normalize a vector while handling 0 length vectors."""
+ norm = numpy.linalg.norm(v)
+ if norm == 0:
+ return v
+ return v / norm
+
+
+# CI is circular index and allows selecting between all the stats that map
+# to the same x-y state (by giving them an integer index).
+# This will compute approximate first and second derivatives with respect
+# to path length.
+def to_theta_with_circular_index_and_derivs(x, y, dx, dy,
+ circular_index_select):
+ a = to_theta_with_circular_index(x, y, circular_index_select)
+ b = to_theta_with_circular_index(x + dx * 0.0001, y + dy * 0.0001,
+ circular_index_select)
+ c = to_theta_with_circular_index(x - dx * 0.0001, y - dy * 0.0001,
+ circular_index_select)
+ d1 = normalize(b - a)
+ d2 = normalize(c - a)
+ accel = (d1 + d2) / numpy.linalg.norm(a - b)
+ return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+def to_theta_with_ci_and_derivs(p_prev, p, p_next, c_i_select):
+ a = to_theta(p, c_i_select)
+ b = to_theta(p_next, c_i_select)
+ c = to_theta(p_prev, c_i_select)
+ d1 = normalize(b - a)
+ d2 = normalize(c - a)
+ accel = (d1 + d2) / numpy.linalg.norm(a - b)
+ return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+# Generic subdivision algorithm.
+def subdivide(p1, p2, max_dist):
+ dx = p2[0] - p1[0]
+ dy = p2[1] - p1[1]
+ dist = numpy.sqrt(dx**2 + dy**2)
+ n = int(numpy.ceil(dist / max_dist))
+ return [(alpha_blend(p1[0], p2[0],
+ float(i) / n), alpha_blend(p1[1], p2[1],
+ float(i) / n))
+ for i in range(1, n + 1)]
+
+
+# convert from an xy space loop into a theta loop.
+# All segements are expected go from one "hyper-extension" boundary
+# to another, thus we must go backwards over the "loop" to get a loop in
+# x-y space.
+def to_theta_loop(lines, cross_point=-numpy.pi):
+ out = []
+ last_pt = lines[0]
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist):
+ out.append(to_theta(pt, 0, cross_point))
+ last_pt = n_pt
+ for n_pt in reversed(lines[:-1]):
+ for pt in subdivide(last_pt, n_pt, max_dist):
+ out.append(to_theta(pt, 1, cross_point))
+ last_pt = n_pt
+ return out
+
+
+# Convert a loop (list of line segments) into
+# The name incorrectly suggests that it is cyclic.
+def back_to_xy_loop(lines):
+ out = []
+ last_pt = lines[0]
+ out.append(to_xy(last_pt[0], last_pt[1]))
+ for n_pt in lines[1:]:
+ for pt in subdivide(last_pt, n_pt, max_dist_theta):
+ out.append(to_xy(pt[0], pt[1]))
+ last_pt = n_pt
+
+ return out
+
+
+def spline_eval(start, control1, control2, end, alpha):
+ a = alpha_blend(start, control1, alpha)
+ b = alpha_blend(control1, control2, alpha)
+ c = alpha_blend(control2, end, alpha)
+ return alpha_blend(alpha_blend(a, b, alpha), alpha_blend(b, c, alpha),
+ alpha)
+
+
+def subdivide_spline(start, control1, control2, end):
+ # TODO: pick N based on spline parameters? or otherwise change it to be more evenly spaced?
+ n = 100
+ for i in range(0, n + 1):
+ yield i / float(n)
+
+
+def get_derivs(t_prev, t, t_next):
+ c, a, b = t_prev, t, t_next
+ d1 = normalize(b - a)
+ d2 = normalize(c - a)
+ accel = (d1 + d2) / numpy.linalg.norm(a - b)
+ return (a[0], a[1], d1[0], d1[1], accel[0], accel[1])
+
+
+# Draw lines to cr + stroke.
+def draw_lines(cr, lines):
+ cr.move_to(lines[0][0], lines[0][1])
+ for pt in lines[1:]:
+ cr.line_to(pt[0], pt[1])
+ with px(cr):
+ cr.stroke()
+
+
+# Segment in angle space.
+class AngleSegment:
+
+ def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
+ """Creates an angle segment.
+
+ Args:
+ start: (double, double), The start of the segment in theta1, theta2
+ coordinates in radians
+ end: (double, double), The end of the segment in theta1, theta2
+ coordinates in radians
+ """
+ self.start = start
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "AngleSegment(%s, %s)" % (repr(self.start), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if theta_version:
+ cr.move_to(self.start[0], self.start[1] + theta_end_circle_size)
+ cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.end[0], self.end[1] + theta_end_circle_size)
+ cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.start[0], self.start[1])
+ cr.line_to(self.end[0], self.end[1])
+ else:
+ start_xy = to_xy(self.start[0], self.start[1])
+ end_xy = to_xy(self.end[0], self.end[1])
+ draw_lines(cr, back_to_xy_loop([self.start, self.end]))
+ cr.move_to(start_xy[0] + xy_end_circle_size, start_xy[1])
+ cr.arc(start_xy[0], start_xy[1], xy_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(end_xy[0] + xy_end_circle_size, end_xy[1])
+ cr.arc(end_xy[0], end_xy[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+ def ToThetaPoints(self):
+ dx = self.end[0] - self.start[0]
+ dy = self.end[1] - self.start[1]
+ mag = numpy.hypot(dx, dy)
+ dx /= mag
+ dy /= mag
+
+ return [(self.start[0], self.start[1], dx, dy, 0.0, 0.0),
+ (self.end[0], self.end[1], dx, dy, 0.0, 0.0)]
+
+
+class XYSegment:
+ """Straight line in XY space."""
+
+ def __init__(self, start, end, name=None, alpha_unitizer=None, vmax=None):
+ """Creates an XY segment.
+
+ Args:
+ start: (double, double), The start of the segment in theta1, theta2
+ coordinates in radians
+ end: (double, double), The end of the segment in theta1, theta2
+ coordinates in radians
+ """
+ self.start = start
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "XYSegment(%s, %s)" % (repr(self.start), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if theta_version:
+ theta1, theta2 = self.start
+ circular_index_select = int(
+ numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+
+ ln = [(start[0], start[1]), (end[0], end[1])]
+ draw_lines(cr, [
+ to_theta_with_circular_index(x, y, circular_index_select)
+ for x, y in subdivide_xy(ln)
+ ])
+ cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+ cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+ cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ else:
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+ cr.move_to(start[0], start[1])
+ cr.line_to(end[0], end[1])
+ cr.move_to(start[0] + xy_end_circle_size, start[1])
+ cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+ cr.move_to(end[0] + xy_end_circle_size, end[1])
+ cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+ def ToThetaPoints(self):
+ """ Converts to points in theta space via to_theta_with_circular_index_and_derivs"""
+ theta1, theta2 = self.start
+ circular_index_select = int(
+ numpy.floor((self.start[1] - self.start[0]) / numpy.pi))
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+
+ ln = [(start[0], start[1]), (end[0], end[1])]
+
+ dx = end[0] - start[0]
+ dy = end[1] - start[1]
+ mag = numpy.hypot(dx, dy)
+ dx /= mag
+ dy /= mag
+
+ return [
+ to_theta_with_circular_index_and_derivs(x, y, dx, dy,
+ circular_index_select)
+ for x, y in subdivide_xy(ln, 0.01)
+ ]
+
+
+class SplineSegment:
+
+ def __init__(self,
+ start,
+ control1,
+ control2,
+ end,
+ name=None,
+ alpha_unitizer=None,
+ vmax=None):
+ self.start = start
+ self.control1 = control1
+ self.control2 = control2
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "SplineSegment(%s, %s, %s, %s)" % (repr(
+ self.start), repr(self.control1), repr(
+ self.control2), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if theta_version:
+ c_i_select = get_circular_index(self.start)
+ start = get_xy(self.start)
+ control1 = get_xy(self.control1)
+ control2 = get_xy(self.control2)
+ end = get_xy(self.end)
+
+ draw_lines(cr, [
+ to_theta(spline_eval(start, control1, control2, end, alpha),
+ c_i_select)
+ for alpha in subdivide_spline(start, control1, control2, end)
+ ])
+ cr.move_to(self.start[0] + theta_end_circle_size, self.start[1])
+ cr.arc(self.start[0], self.start[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ cr.move_to(self.end[0] + theta_end_circle_size, self.end[1])
+ cr.arc(self.end[0], self.end[1], theta_end_circle_size, 0,
+ 2.0 * numpy.pi)
+ else:
+ start = get_xy(self.start)
+ control1 = get_xy(self.control1)
+ control2 = get_xy(self.control2)
+ end = get_xy(self.end)
+
+ draw_lines(cr, [
+ spline_eval(start, control1, control2, end, alpha)
+ for alpha in subdivide_spline(start, control1, control2, end)
+ ])
+
+ cr.move_to(start[0] + xy_end_circle_size, start[1])
+ cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+ cr.move_to(end[0] + xy_end_circle_size, end[1])
+ cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+ def ToThetaPoints(self):
+ t1, t2 = self.start
+ c_i_select = get_circular_index(self.start)
+ start = get_xy(self.start)
+ control1 = get_xy(self.control1)
+ control2 = get_xy(self.control2)
+ end = get_xy(self.end)
+
+ return [
+ to_theta_with_ci_and_derivs(
+ spline_eval(start, control1, control2, end, alpha - 0.00001),
+ spline_eval(start, control1, control2, end, alpha),
+ spline_eval(start, control1, control2, end, alpha + 0.00001),
+ c_i_select)
+ for alpha in subdivide_spline(start, control1, control2, end)
+ ]
+
+
+class ThetaSplineSegment:
+
+ def __init__(self,
+ start,
+ control1,
+ control2,
+ end,
+ name=None,
+ alpha_unitizer=None,
+ vmax=None):
+ self.start = start
+ self.control1 = control1
+ self.control2 = control2
+ self.end = end
+ self.name = name
+ self.alpha_unitizer = alpha_unitizer
+ self.vmax = vmax
+
+ def __repr__(self):
+ return "ThetaSplineSegment(%s, %s, &s, %s)" % (repr(
+ self.start), repr(self.control1), repr(
+ self.control2), repr(self.end))
+
+ def DrawTo(self, cr, theta_version):
+ if (theta_version):
+ draw_lines(cr, [
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha)
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ])
+ else:
+ start = get_xy(self.start)
+ end = get_xy(self.end)
+
+ draw_lines(cr, [
+ get_xy(
+ spline_eval(self.start, self.control1, self.control2,
+ self.end, alpha))
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ])
+
+ cr.move_to(start[0] + xy_end_circle_size, start[1])
+ cr.arc(start[0], start[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+ cr.move_to(end[0] + xy_end_circle_size, end[1])
+ cr.arc(end[0], end[1], xy_end_circle_size, 0, 2.0 * numpy.pi)
+
+ def ToThetaPoints(self):
+ return [
+ get_derivs(
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha - 0.00001),
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha),
+ spline_eval(self.start, self.control1, self.control2, self.end,
+ alpha + 0.00001))
+ for alpha in subdivide_spline(self.start, self.control1,
+ self.control2, self.end)
+ ]
+
+
+def expand_points(points, max_distance):
+ """Expands a list of points to be at most max_distance apart
+
+ Generates the paths to connect the new points to the closest input points,
+ and the paths connecting the points.
+
+ Args:
+ points, list of tuple of point, name, The points to start with and fill
+ in.
+ max_distance, float, The max distance between two points when expanding
+ the graph.
+
+ Return:
+ points, edges
+ """
+ result_points = [points[0]]
+ result_paths = []
+ for point, name in points[1:]:
+ previous_point = result_points[-1][0]
+ previous_point_xy = get_xy(previous_point)
+ circular_index = get_circular_index(previous_point)
+
+ point_xy = get_xy(point)
+ norm = numpy.linalg.norm(point_xy - previous_point_xy)
+ num_points = int(numpy.ceil(norm / max_distance))
+ last_iteration_point = previous_point
+ for subindex in range(1, num_points):
+ subpoint = to_theta(alpha_blend(previous_point_xy, point_xy,
+ float(subindex) / num_points),
+ circular_index=circular_index)
+ result_points.append(
+ (subpoint, '%s%dof%d' % (name, subindex, num_points)))
+ result_paths.append(
+ XYSegment(last_iteration_point, subpoint, vmax=6.0))
+ if (last_iteration_point != previous_point).any():
+ result_paths.append(XYSegment(previous_point, subpoint))
+ if subindex == num_points - 1:
+ result_paths.append(XYSegment(subpoint, point, vmax=6.0))
+ else:
+ result_paths.append(XYSegment(subpoint, point))
+ last_iteration_point = subpoint
+ result_points.append((point, name))
+
+ return result_points, result_paths
diff --git a/y2023/rockpi/BUILD b/y2023/rockpi/BUILD
new file mode 100644
index 0000000..91e8729
--- /dev/null
+++ b/y2023/rockpi/BUILD
@@ -0,0 +1,11 @@
+cc_binary(
+ name = "imu_main",
+ srcs = ["imu_main.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/imu_reader:imu",
+ ],
+)
diff --git a/y2023/rockpi/imu_main.cc b/y2023/rockpi/imu_main.cc
new file mode 100644
index 0000000..ac0c141
--- /dev/null
+++ b/y2023/rockpi/imu_main.cc
@@ -0,0 +1,24 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/realtime.h"
+#include "frc971/imu_reader/imu.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+ // TODO(austin): Set the ratio...
+ frc971::imu::Imu imu(&event_loop, 1.0);
+
+ event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+ event_loop.SetRuntimeRealtimePriority(55);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 2f93b16..2ddb735 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -1,9 +1,56 @@
+py_binary(
+ name = "create_calib_file",
+ srcs = [
+ "create_calib_file.py",
+ ],
+ args = [
+ "calibration_data.h",
+ ],
+ data = glob(["calib_files/*.json"]),
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/vision:create_calib_file",
+ ],
+)
+
+genrule(
+ name = "run_calibration_data",
+ outs = [
+ "calibration_data.h",
+ ],
+ cmd = " ".join([
+ "$(location :create_calib_file)",
+ "$(location calibration_data.h)",
+ ]),
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ ":create_calib_file",
+ ],
+)
+
+cc_library(
+ name = "calibration_data",
+ hdrs = [
+ "calibration_data.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "@com_google_absl//absl/types:span",
+ ],
+)
+
cc_binary(
name = "camera_reader",
srcs = [
"camera_reader.cc",
+ "rkisp1-config.h",
],
- target_compatible_with = ["@platforms//os:linux"],
+ target_compatible_with = [
+ "@platforms//os:linux",
+ "//tools/platforms/hardware:raspberry_pi",
+ ],
visibility = ["//y2023:__subpackages__"],
deps = [
"//aos:init",
@@ -22,8 +69,70 @@
visibility = ["//y2023:__subpackages__"],
deps = [
"//aos:init",
+ "//aos:json_to_flatbuffer",
"//aos/events:shm_event_loop",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
+ "@com_google_absl//absl/strings",
+ ],
+)
+
+cc_binary(
+ name = "target_mapping",
+ srcs = [
+ "target_mapping.cc",
+ ],
+ data = [
+ "//y2023:aos_config",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ ":aprilrobotics_lib",
+ ":calibration_data",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//frc971/control_loops:pose",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:target_mapper",
+ "//third_party:opencv",
+ ],
+)
+
+cc_library(
+ name = "aprilrobotics_lib",
+ srcs = [
+ "aprilrobotics.cc",
+ "aprilrobotics.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ ":calibration_data",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:target_map_fbs",
+ "//frc971/vision:target_mapper",
+ "//frc971/vision:vision_fbs",
+ "//third_party:opencv",
+ "//third_party/apriltag",
+ ],
+)
+
+cc_binary(
+ name = "aprilrobotics",
+ srcs = [
+ "aprilrobotics_main.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ ":aprilrobotics_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
new file mode 100644
index 0000000..cbfd3f6
--- /dev/null
+++ b/y2023/vision/aprilrobotics.cc
@@ -0,0 +1,178 @@
+#include "y2023/vision/aprilrobotics.h"
+
+DEFINE_bool(
+ debug, false,
+ "If true, dump a ton of debug and crash on the first valid detection.");
+
+DEFINE_int32(team_number, 971,
+ "Use the calibration for a node with this team number");
+namespace y2023 {
+namespace vision {
+
+namespace chrono = std::chrono;
+
+AprilRoboticsDetector::AprilRoboticsDetector(aos::EventLoop *event_loop,
+ std::string_view channel_name)
+ : calibration_data_(CalibrationData()),
+ ftrace_(),
+ image_callback_(
+ event_loop, channel_name,
+ [&](cv::Mat image_color_mat,
+ const aos::monotonic_clock::time_point eof) {
+ HandleImage(image_color_mat, eof);
+ },
+ chrono::milliseconds(5)),
+ target_map_sender_(
+ event_loop->MakeSender<frc971::vision::TargetMap>("/camera")) {
+ tag_family_ = tag16h5_create();
+ tag_detector_ = apriltag_detector_create();
+
+ apriltag_detector_add_family_bits(tag_detector_, tag_family_, 1);
+ tag_detector_->nthreads = 6;
+ tag_detector_->wp = workerpool_create(tag_detector_->nthreads);
+ tag_detector_->qtp.min_white_black_diff = 5;
+ tag_detector_->debug = FLAGS_debug;
+
+ std::string hostname = aos::network::GetHostname();
+
+ // Check team string is valid
+ std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(hostname);
+ std::optional<uint16_t> team_number =
+ aos::network::team_number_internal::ParsePiTeamNumber(hostname);
+ CHECK(pi_number) << "Unable to parse pi number from '" << hostname << "'";
+ CHECK(team_number);
+
+ calibration_ = FindCameraCalibration(&calibration_data_.message(),
+ "pi" + std::to_string(*pi_number));
+ intrinsics_ = CameraIntrinsics(calibration_);
+ camera_distortion_coeffs_ = CameraDistCoeffs(calibration_);
+
+ image_callback_.set_format(frc971::vision::ImageCallback::Format::GRAYSCALE);
+}
+
+AprilRoboticsDetector::~AprilRoboticsDetector() {
+ apriltag_detector_destroy(tag_detector_);
+ free(tag_family_);
+}
+
+void AprilRoboticsDetector::SetWorkerpoolAffinities() {
+ for (int i = 0; i < tag_detector_->wp->nthreads; i++) {
+ cpu_set_t affinity;
+ CPU_ZERO(&affinity);
+ CPU_SET(i, &affinity);
+ pthread_setaffinity_np(tag_detector_->wp->threads[i], sizeof(affinity),
+ &affinity);
+ struct sched_param param;
+ param.sched_priority = 20;
+ int res = pthread_setschedparam(tag_detector_->wp->threads[i], SCHED_FIFO,
+ ¶m);
+ PCHECK(res == 0) << "Failed to set priority of threadpool threads";
+ }
+}
+
+void AprilRoboticsDetector::HandleImage(cv::Mat image_color_mat,
+ aos::monotonic_clock::time_point eof) {
+ std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> detections =
+ DetectTags(image_color_mat);
+
+ auto builder = target_map_sender_.MakeBuilder();
+ std::vector<flatbuffers::Offset<frc971::vision::TargetPoseFbs>> target_poses;
+ for (const auto &[detection, pose] : detections) {
+ target_poses.emplace_back(
+ BuildTargetPose(pose, detection.id, builder.fbb()));
+ }
+ const auto target_poses_offset = builder.fbb()->CreateVector(target_poses);
+ auto target_map_builder = builder.MakeBuilder<frc971::vision::TargetMap>();
+ target_map_builder.add_target_poses(target_poses_offset);
+ target_map_builder.add_monotonic_timestamp_ns(eof.time_since_epoch().count());
+ builder.CheckOk(builder.Send(target_map_builder.Finish()));
+}
+
+flatbuffers::Offset<frc971::vision::TargetPoseFbs>
+AprilRoboticsDetector::BuildTargetPose(
+ const apriltag_pose_t &pose,
+ frc971::vision::TargetMapper::TargetId target_id,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const auto T =
+ Eigen::Translation3d(pose.t->data[0], pose.t->data[1], pose.t->data[2]);
+ const auto position_offset =
+ frc971::vision::CreatePosition(*fbb, T.x(), T.y(), T.z());
+
+ const auto orientation = Eigen::Quaterniond(Eigen::Matrix3d(pose.R->data));
+ const auto orientation_offset = frc971::vision::CreateQuaternion(
+ *fbb, orientation.w(), orientation.x(), orientation.y(), orientation.z());
+
+ return frc971::vision::CreateTargetPoseFbs(*fbb, target_id, position_offset,
+ orientation_offset);
+}
+
+std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>>
+AprilRoboticsDetector::DetectTags(cv::Mat image) {
+ const aos::monotonic_clock::time_point start_time =
+ aos::monotonic_clock::now();
+
+ image_u8_t im = {
+ .width = image.cols,
+ .height = image.rows,
+ .stride = image.cols,
+ .buf = image.data,
+ };
+
+ ftrace_.FormatMessage("Starting detect\n");
+ zarray_t *detections = apriltag_detector_detect(tag_detector_, &im);
+ ftrace_.FormatMessage("Done detecting\n");
+
+ std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
+
+ for (int i = 0; i < zarray_size(detections); i++) {
+ apriltag_detection_t *det;
+ zarray_get(detections, i, &det);
+
+ if (det->decision_margin > 30) {
+ VLOG(1) << "Found tag number " << det->id << " hamming: " << det->hamming
+ << " margin: " << det->decision_margin;
+
+ const aos::monotonic_clock::time_point before_pose_estimation =
+ aos::monotonic_clock::now();
+ // First create an apriltag_detection_info_t struct using your known
+ // parameters.
+ apriltag_detection_info_t info;
+ info.det = det;
+ info.tagsize = 0.1524;
+ info.fx = intrinsics_.at<double>(0, 0);
+ info.fy = intrinsics_.at<double>(1, 1);
+ info.cx = intrinsics_.at<double>(0, 2);
+ info.cy = intrinsics_.at<double>(1, 2);
+
+ apriltag_pose_t pose;
+ double err = estimate_tag_pose(&info, &pose);
+
+ VLOG(1) << "err: " << err;
+
+ results.emplace_back(*det, pose);
+
+ const aos::monotonic_clock::time_point after_pose_estimation =
+ aos::monotonic_clock::now();
+
+ VLOG(1) << "Took "
+ << chrono::duration<double>(after_pose_estimation -
+ before_pose_estimation)
+ .count()
+ << " seconds for pose estimation";
+ }
+ }
+
+ apriltag_detections_destroy(detections);
+
+ const aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
+
+ timeprofile_display(tag_detector_->tp);
+
+ VLOG(1) << "Took " << chrono::duration<double>(end_time - start_time).count()
+ << " seconds to detect overall";
+
+ return results;
+}
+
+} // namespace vision
+} // namespace y2023
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
new file mode 100644
index 0000000..a68b1d9
--- /dev/null
+++ b/y2023/vision/aprilrobotics.h
@@ -0,0 +1,101 @@
+
+#include <string>
+
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/network/team_number.h"
+#include "aos/realtime.h"
+#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_generated.h"
+#include "opencv2/core/eigen.hpp"
+#include "opencv2/imgproc.hpp"
+#include "third_party/apriltag/apriltag.h"
+#include "third_party/apriltag/apriltag_pose.h"
+#include "third_party/apriltag/tag16h5.h"
+#include "y2023/vision/calibration_data.h"
+
+DECLARE_int32(team_number);
+
+namespace y2023 {
+namespace vision {
+
+class AprilRoboticsDetector {
+ public:
+ AprilRoboticsDetector(aos::EventLoop *event_loop,
+ std::string_view channel_name);
+
+ ~AprilRoboticsDetector();
+
+ void SetWorkerpoolAffinities();
+
+ std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> DetectTags(
+ cv::Mat image);
+
+ private:
+ void HandleImage(cv::Mat image, aos::monotonic_clock::time_point eof);
+
+ flatbuffers::Offset<frc971::vision::TargetPoseFbs> BuildTargetPose(
+ const apriltag_pose_t &pose,
+ frc971::vision::TargetMapper::TargetId target_id,
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ static const frc971::vision::calibration::CameraCalibration *
+ FindCameraCalibration(
+ const frc971::vision::calibration::CalibrationData *calibration_data,
+ std::string_view node_name) {
+ for (const frc971::vision::calibration::CameraCalibration *candidate :
+ *calibration_data->camera_calibrations()) {
+ if (candidate->node_name()->string_view() != node_name) {
+ continue;
+ }
+ if (candidate->team_number() != FLAGS_team_number) {
+ continue;
+ }
+ return candidate;
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+ << " on " << FLAGS_team_number;
+ }
+
+ static cv::Mat CameraIntrinsics(
+ const frc971::vision::calibration::CameraCalibration
+ *camera_calibration) {
+ cv::Mat result(3, 3, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->intrinsics()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(), camera_calibration->intrinsics()->size());
+
+ return result;
+ }
+
+ static cv::Mat CameraDistCoeffs(
+ const frc971::vision::calibration::CameraCalibration
+ *camera_calibration) {
+ const cv::Mat result(5, 1, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->dist_coeffs()->data())));
+ CHECK_EQ(result.total(), camera_calibration->dist_coeffs()->size());
+ return result;
+ }
+
+ apriltag_family_t *tag_family_;
+ apriltag_detector_t *tag_detector_;
+
+ const aos::FlatbufferSpan<frc971::vision::calibration::CalibrationData>
+ calibration_data_;
+ const frc971::vision::calibration::CameraCalibration *calibration_;
+ cv::Mat intrinsics_;
+ cv::Mat camera_distortion_coeffs_;
+
+ aos::Ftrace ftrace_;
+
+ frc971::vision::ImageCallback image_callback_;
+ aos::Sender<frc971::vision::TargetMap> target_map_sender_;
+};
+
+} // namespace vision
+} // namespace y2023
diff --git a/y2023/vision/aprilrobotics_main.cc b/y2023/vision/aprilrobotics_main.cc
new file mode 100644
index 0000000..67b853f
--- /dev/null
+++ b/y2023/vision/aprilrobotics_main.cc
@@ -0,0 +1,34 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2023/vision/aprilrobotics.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+namespace y2023::vision {
+void AprilViewerMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ AprilRoboticsDetector detector(&event_loop, "/camera");
+
+ detector.SetWorkerpoolAffinities();
+
+ event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({5}));
+
+ struct sched_param param;
+ param.sched_priority = 21;
+ PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0);
+
+ event_loop.Run();
+}
+
+} // namespace y2023::vision
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2023::vision::AprilViewerMain();
+
+ return 0;
+}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json b/y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json
new file mode 100755
index 0000000..a8d8816
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-7971-1_2021-06-12_15-35-39.636386620.json
@@ -0,0 +1,41 @@
+{
+ "node_name": "pi1",
+ "team_number": 7971,
+ "intrinsics": [
+ 388.369812,
+ 0.0,
+ 292.325653,
+ 0.0,
+ 388.513733,
+ 224.371063,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.126935,
+ -0.218447,
+ -0.000152,
+ 0.001158,
+ 0.06266
+ ],
+ "fixed_extrinsics": [
+ -1.0,
+ -1.57586107256918e-16,
+ 5.0158596452676243e-17,
+ -0.15239999999999998,
+ 1.3147519464173305e-16,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ -0.2032,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "calibration_timestamp": 1623537339636386620
+}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json b/y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json
new file mode 100755
index 0000000..d994a07
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-7971-2_2021-06-12_15-30-20.325393444.json
@@ -0,0 +1,41 @@
+{
+ "node_name": "pi2",
+ "team_number": 7971,
+ "intrinsics": [
+ 388.7565,
+ 0.0,
+ 285.024506,
+ 0.0,
+ 388.915039,
+ 222.227539,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.128415,
+ -0.212528,
+ 0.001165,
+ 0.000579,
+ 0.054853
+ ],
+ "fixed_extrinsics": [
+ 7.02428546843654e-17,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ 0.09525,
+ 1.0,
+ 1.2246467991473532e-16,
+ 0.0,
+ 0.1905,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "calibration_timestamp": 1623537020325393444
+}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json b/y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json
new file mode 100755
index 0000000..241957e
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-7971-3_2021-06-12_15-33-31.977365877.json
@@ -0,0 +1,41 @@
+{
+ "node_name": "pi3",
+ "team_number": 7971,
+ "intrinsics": [
+ 389.35611,
+ 0.0,
+ 339.345673,
+ 0.0,
+ 389.516235,
+ 240.247787,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.122511,
+ -0.209383,
+ -0.001212,
+ 0.000041,
+ 0.05674
+ ],
+ "fixed_extrinsics": [
+ 7.02428546843654e-17,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ 0.09525,
+ 1.0,
+ 1.2246467991473532e-16,
+ 0.0,
+ -0.10794999999999999,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "calibration_timestamp": 1623537211977365877
+}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json b/y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json
new file mode 100755
index 0000000..6e04089
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-7971-4_2021-06-12_15-37-25.706564865.json
@@ -0,0 +1,41 @@
+{
+ "node_name": "pi4",
+ "team_number": 7971,
+ "intrinsics": [
+ 390.301514,
+ 0.0,
+ 356.104095,
+ 0.0,
+ 389.884491,
+ 231.157303,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.128595,
+ -0.229324,
+ -0.001145,
+ 0.001602,
+ 0.079774
+ ],
+ "fixed_extrinsics": [
+ 7.02428546843654e-17,
+ -0.5735764363510459,
+ 0.8191520442889919,
+ -0.15239999999999998,
+ 1.0,
+ 1.2246467991473532e-16,
+ 0.0,
+ -0.17779999999999999,
+ -1.0031719290535249e-16,
+ 0.8191520442889919,
+ 0.5735764363510459,
+ 0.0127,
+ 0.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "calibration_timestamp": 1623537445706564865
+}
diff --git a/y2023/vision/camera_reader.cc b/y2023/vision/camera_reader.cc
index 560e9ee..b604516 100644
--- a/y2023/vision/camera_reader.cc
+++ b/y2023/vision/camera_reader.cc
@@ -1,3 +1,6 @@
+#include <linux/videodev2.h>
+#include <sys/ioctl.h>
+
#include "absl/strings/str_cat.h"
#include "absl/strings/str_split.h"
#include "aos/events/shm_event_loop.h"
@@ -5,9 +8,15 @@
#include "aos/realtime.h"
#include "frc971/vision/media_device.h"
#include "frc971/vision/v4l2_reader.h"
+#include "y2023/vision/rkisp1-config.h"
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
-DEFINE_bool(lowlight_camera, false, "Switch to use imx462 image sensor.");
+DEFINE_bool(lowlight_camera, true, "Switch to use imx462 image sensor.");
+
+DEFINE_double(red, 1.252, "Red gain");
+DEFINE_double(green, 1, "Green gain");
+DEFINE_double(blue, 1.96, "Blue gain");
+DEFINE_double(exposure, 150, "Camera exposure");
namespace y2023 {
namespace vision {
@@ -30,7 +39,7 @@
width = 1920;
height = 1080;
color_format = MEDIA_BUS_FMT_SRGGB10_1X10;
- camera_device_string = "imx290 4-0036";
+ camera_device_string = "arducam-pivariety 4-000c";
}
media_device->Reset();
@@ -54,8 +63,8 @@
media_device->FindEntity("rkisp1_resizer_selfpath");
rkisp1_resizer_selfpath->pads(0)->SetSubdevFormat(width, height,
MEDIA_BUS_FMT_YUYV8_2X8);
- rkisp1_resizer_selfpath->pads(1)->SetSubdevFormat(width, height,
- MEDIA_BUS_FMT_YUYV8_2X8);
+ rkisp1_resizer_selfpath->pads(1)->SetSubdevFormat(
+ width * 2 / 3, height * 2 / 3, MEDIA_BUS_FMT_YUYV8_2X8);
rkisp1_resizer_selfpath->pads(0)->SetSubdevCrop(width, height);
Entity *rkisp1_resizer_mainpath =
@@ -71,7 +80,7 @@
rkisp1_mainpath->SetFormat(width / 2, height / 2, V4L2_PIX_FMT_YUV422P);
Entity *rkisp1_selfpath = media_device->FindEntity("rkisp1_selfpath");
- rkisp1_selfpath->SetFormat(width, height, V4L2_PIX_FMT_YUYV);
+ rkisp1_selfpath->SetFormat(width * 2 / 3, height * 2 / 3, V4L2_PIX_FMT_YUYV);
media_device->Enable(
media_device->FindLink(camera_device_string, 0, "rkisp1_csi", 0));
@@ -94,14 +103,77 @@
rkisp1_selfpath->device(), camera->device());
if (FLAGS_lowlight_camera) {
- v4l2_reader.SetGain(72);
- v4l2_reader.SetExposure(30);
- v4l2_reader.SetBlanking(2480, 45);
+ v4l2_reader.SetGainExt(100);
+ v4l2_reader.SetVerticalBlanking(1000);
+ v4l2_reader.SetExposure(FLAGS_exposure);
} else {
v4l2_reader.SetGainExt(1000);
v4l2_reader.SetExposure(1000);
}
+ {
+ Entity *rkisp1_params = media_device->FindEntity("rkisp1_params");
+
+ LOG(INFO) << "Opening " << rkisp1_params->device();
+ aos::ScopedFD fd(open(rkisp1_params->device().c_str(), O_RDWR));
+ PCHECK(fd >= 0);
+
+ struct v4l2_capability capability;
+ memset(&capability, 0, sizeof(capability));
+ PCHECK(ioctl(fd.get(), VIDIOC_QUERYCAP, &capability) == 0);
+ CHECK(capability.device_caps & V4L2_CAP_META_OUTPUT);
+
+ // V4L2_META_FMT_RK_ISP1_PARAMS
+ // RK1P
+ uint32_t meta_params_format = (uint32_t)('R') | ((uint32_t)('K') << 8) |
+ ((uint32_t)('1') << 16) |
+ ((uint32_t)('P') << 24);
+ struct v4l2_format format;
+ std::memset(&format, 0, sizeof(format));
+ format.type = V4L2_BUF_TYPE_META_OUTPUT;
+
+ PCHECK(ioctl(fd.get(), VIDIOC_G_FMT, &format) == 0);
+ CHECK_EQ(format.fmt.meta.buffersize, 3048ul);
+ CHECK_EQ(format.fmt.meta.dataformat, meta_params_format);
+
+ struct v4l2_requestbuffers request;
+ memset(&request, 0, sizeof(request));
+ request.count = 1;
+ request.type = V4L2_BUF_TYPE_META_OUTPUT;
+ request.memory = V4L2_MEMORY_USERPTR;
+ PCHECK(ioctl(fd.get(), VIDIOC_REQBUFS, &request) == 0);
+
+ struct rkisp1_params_cfg configuration;
+ memset(&configuration, 0, sizeof(configuration));
+
+ configuration.module_cfg_update |= RKISP1_CIF_ISP_MODULE_AWB_GAIN;
+
+ configuration.others.awb_gain_config.gain_red = 256 * FLAGS_red;
+ configuration.others.awb_gain_config.gain_green_r = 256 * FLAGS_green;
+ configuration.others.awb_gain_config.gain_blue = 256 * FLAGS_blue;
+ configuration.others.awb_gain_config.gain_green_b = 256 * FLAGS_green;
+
+ // Enable the AWB gains
+ configuration.module_en_update |= RKISP1_CIF_ISP_MODULE_AWB_GAIN;
+ configuration.module_ens |= RKISP1_CIF_ISP_MODULE_AWB_GAIN;
+
+ struct v4l2_buffer buffer;
+ memset(&buffer, 0, sizeof(buffer));
+ buffer.memory = V4L2_MEMORY_USERPTR;
+ buffer.index = 0;
+ buffer.type = V4L2_BUF_TYPE_META_OUTPUT;
+ buffer.m.userptr = reinterpret_cast<uintptr_t>(&configuration);
+ buffer.length = format.fmt.meta.buffersize;
+
+ int type = V4L2_BUF_TYPE_META_OUTPUT;
+ PCHECK(ioctl(fd.get(), VIDIOC_STREAMON, &type) == 0);
+
+ PCHECK(ioctl(fd.get(), VIDIOC_QBUF, &buffer) == 0);
+ CHECK(buffer.flags & V4L2_BUF_FLAG_QUEUED);
+
+ PCHECK(ioctl(fd.get(), VIDIOC_DQBUF, &buffer) == 0);
+ }
+
event_loop.Run();
}
diff --git a/y2023/vision/create_calib_file.py b/y2023/vision/create_calib_file.py
new file mode 100644
index 0000000..b5e620d
--- /dev/null
+++ b/y2023/vision/create_calib_file.py
@@ -0,0 +1,4 @@
+import frc971.vision.create_calib_file
+
+if __name__ == "__main__":
+ frc971.vision.create_calib_file.generate_header("2023")
diff --git a/y2023/vision/rkisp1-config.h b/y2023/vision/rkisp1-config.h
new file mode 100644
index 0000000..583ca0d
--- /dev/null
+++ b/y2023/vision/rkisp1-config.h
@@ -0,0 +1,950 @@
+/* SPDX-License-Identifier: ((GPL-2.0+ WITH Linux-syscall-note) OR MIT) */
+/*
+ * Rockchip ISP1 userspace API
+ * Copyright (C) 2017 Rockchip Electronics Co., Ltd.
+ */
+
+#ifndef _UAPI_RKISP1_CONFIG_H
+#define _UAPI_RKISP1_CONFIG_H
+
+#include <linux/types.h>
+
+/* Defect Pixel Cluster Detection */
+#define RKISP1_CIF_ISP_MODULE_DPCC (1U << 0)
+/* Black Level Subtraction */
+#define RKISP1_CIF_ISP_MODULE_BLS (1U << 1)
+/* Sensor De-gamma */
+#define RKISP1_CIF_ISP_MODULE_SDG (1U << 2)
+/* Histogram statistics configuration */
+#define RKISP1_CIF_ISP_MODULE_HST (1U << 3)
+/* Lens Shade Control */
+#define RKISP1_CIF_ISP_MODULE_LSC (1U << 4)
+/* Auto White Balance Gain */
+#define RKISP1_CIF_ISP_MODULE_AWB_GAIN (1U << 5)
+/* Filter */
+#define RKISP1_CIF_ISP_MODULE_FLT (1U << 6)
+/* Bayer Demosaic */
+#define RKISP1_CIF_ISP_MODULE_BDM (1U << 7)
+/* Cross Talk */
+#define RKISP1_CIF_ISP_MODULE_CTK (1U << 8)
+/* Gamma Out Curve */
+#define RKISP1_CIF_ISP_MODULE_GOC (1U << 9)
+/* Color Processing */
+#define RKISP1_CIF_ISP_MODULE_CPROC (1U << 10)
+/* Auto Focus Control statistics configuration */
+#define RKISP1_CIF_ISP_MODULE_AFC (1U << 11)
+/* Auto White Balancing statistics configuration */
+#define RKISP1_CIF_ISP_MODULE_AWB (1U << 12)
+/* Image Effect */
+#define RKISP1_CIF_ISP_MODULE_IE (1U << 13)
+/* Auto Exposure Control statistics configuration */
+#define RKISP1_CIF_ISP_MODULE_AEC (1U << 14)
+/* Wide Dynamic Range */
+#define RKISP1_CIF_ISP_MODULE_WDR (1U << 15)
+/* Denoise Pre-Filter */
+#define RKISP1_CIF_ISP_MODULE_DPF (1U << 16)
+/* Denoise Pre-Filter Strength */
+#define RKISP1_CIF_ISP_MODULE_DPF_STRENGTH (1U << 17)
+
+#define RKISP1_CIF_ISP_CTK_COEFF_MAX 0x100
+#define RKISP1_CIF_ISP_CTK_OFFSET_MAX 0x800
+
+#define RKISP1_CIF_ISP_AE_MEAN_MAX_V10 25
+#define RKISP1_CIF_ISP_AE_MEAN_MAX_V12 81
+#define RKISP1_CIF_ISP_AE_MEAN_MAX RKISP1_CIF_ISP_AE_MEAN_MAX_V12
+
+#define RKISP1_CIF_ISP_HIST_BIN_N_MAX_V10 16
+#define RKISP1_CIF_ISP_HIST_BIN_N_MAX_V12 32
+#define RKISP1_CIF_ISP_HIST_BIN_N_MAX RKISP1_CIF_ISP_HIST_BIN_N_MAX_V12
+
+#define RKISP1_CIF_ISP_AFM_MAX_WINDOWS 3
+#define RKISP1_CIF_ISP_DEGAMMA_CURVE_SIZE 17
+
+#define RKISP1_CIF_ISP_BDM_MAX_TH 0xff
+
+/*
+ * Black level compensation
+ */
+/* maximum value for horizontal start address */
+#define RKISP1_CIF_ISP_BLS_START_H_MAX 0x00000fff
+/* maximum value for horizontal stop address */
+#define RKISP1_CIF_ISP_BLS_STOP_H_MAX 0x00000fff
+/* maximum value for vertical start address */
+#define RKISP1_CIF_ISP_BLS_START_V_MAX 0x00000fff
+/* maximum value for vertical stop address */
+#define RKISP1_CIF_ISP_BLS_STOP_V_MAX 0x00000fff
+/* maximum is 2^18 = 262144*/
+#define RKISP1_CIF_ISP_BLS_SAMPLES_MAX 0x00000012
+/* maximum value for fixed black level */
+#define RKISP1_CIF_ISP_BLS_FIX_SUB_MAX 0x00000fff
+/* minimum value for fixed black level */
+#define RKISP1_CIF_ISP_BLS_FIX_SUB_MIN 0xfffff000
+/* 13 bit range (signed)*/
+#define RKISP1_CIF_ISP_BLS_FIX_MASK 0x00001fff
+
+/*
+ * Automatic white balance measurements
+ */
+#define RKISP1_CIF_ISP_AWB_MAX_GRID 1
+#define RKISP1_CIF_ISP_AWB_MAX_FRAMES 7
+
+/*
+ * Gamma out
+ */
+/* Maximum number of color samples supported */
+#define RKISP1_CIF_ISP_GAMMA_OUT_MAX_SAMPLES_V10 17
+#define RKISP1_CIF_ISP_GAMMA_OUT_MAX_SAMPLES_V12 34
+#define RKISP1_CIF_ISP_GAMMA_OUT_MAX_SAMPLES RKISP1_CIF_ISP_GAMMA_OUT_MAX_SAMPLES_V12
+
+/*
+ * Lens shade correction
+ */
+#define RKISP1_CIF_ISP_LSC_SECTORS_TBL_SIZE 8
+
+/*
+ * The following matches the tuning process,
+ * not the max capabilities of the chip.
+ */
+#define RKISP1_CIF_ISP_LSC_SAMPLES_MAX 17
+
+/*
+ * Histogram calculation
+ */
+#define RKISP1_CIF_ISP_HISTOGRAM_WEIGHT_GRIDS_SIZE_V10 25
+#define RKISP1_CIF_ISP_HISTOGRAM_WEIGHT_GRIDS_SIZE_V12 81
+#define RKISP1_CIF_ISP_HISTOGRAM_WEIGHT_GRIDS_SIZE RKISP1_CIF_ISP_HISTOGRAM_WEIGHT_GRIDS_SIZE_V12
+
+/*
+ * Defect Pixel Cluster Correction
+ */
+#define RKISP1_CIF_ISP_DPCC_METHODS_MAX 3
+
+/*
+ * Denoising pre filter
+ */
+#define RKISP1_CIF_ISP_DPF_MAX_NLF_COEFFS 17
+#define RKISP1_CIF_ISP_DPF_MAX_SPATIAL_COEFFS 6
+
+/*
+ * Measurement types
+ */
+#define RKISP1_CIF_ISP_STAT_AWB (1U << 0)
+#define RKISP1_CIF_ISP_STAT_AUTOEXP (1U << 1)
+#define RKISP1_CIF_ISP_STAT_AFM (1U << 2)
+#define RKISP1_CIF_ISP_STAT_HIST (1U << 3)
+
+/**
+ * enum rkisp1_cif_isp_version - ISP variants
+ *
+ * @RKISP1_V10: used at least in rk3288 and rk3399
+ * @RKISP1_V11: declared in the original vendor code, but not used
+ * @RKISP1_V12: used at least in rk3326 and px30
+ * @RKISP1_V13: used at least in rk1808
+ */
+enum rkisp1_cif_isp_version {
+ RKISP1_V10 = 10,
+ RKISP1_V11,
+ RKISP1_V12,
+ RKISP1_V13,
+};
+
+enum rkisp1_cif_isp_histogram_mode {
+ RKISP1_CIF_ISP_HISTOGRAM_MODE_DISABLE,
+ RKISP1_CIF_ISP_HISTOGRAM_MODE_RGB_COMBINED,
+ RKISP1_CIF_ISP_HISTOGRAM_MODE_R_HISTOGRAM,
+ RKISP1_CIF_ISP_HISTOGRAM_MODE_G_HISTOGRAM,
+ RKISP1_CIF_ISP_HISTOGRAM_MODE_B_HISTOGRAM,
+ RKISP1_CIF_ISP_HISTOGRAM_MODE_Y_HISTOGRAM
+};
+
+enum rkisp1_cif_isp_awb_mode_type {
+ RKISP1_CIF_ISP_AWB_MODE_MANUAL,
+ RKISP1_CIF_ISP_AWB_MODE_RGB,
+ RKISP1_CIF_ISP_AWB_MODE_YCBCR
+};
+
+enum rkisp1_cif_isp_flt_mode {
+ RKISP1_CIF_ISP_FLT_STATIC_MODE,
+ RKISP1_CIF_ISP_FLT_DYNAMIC_MODE
+};
+
+/**
+ * enum rkisp1_cif_isp_exp_ctrl_autostop - stop modes
+ * @RKISP1_CIF_ISP_EXP_CTRL_AUTOSTOP_0: continuous measurement
+ * @RKISP1_CIF_ISP_EXP_CTRL_AUTOSTOP_1: stop measuring after a complete frame
+ */
+enum rkisp1_cif_isp_exp_ctrl_autostop {
+ RKISP1_CIF_ISP_EXP_CTRL_AUTOSTOP_0 = 0,
+ RKISP1_CIF_ISP_EXP_CTRL_AUTOSTOP_1 = 1,
+};
+
+/**
+ * enum rkisp1_cif_isp_exp_meas_mode - Exposure measure mode
+ * @RKISP1_CIF_ISP_EXP_MEASURING_MODE_0: Y = 16 + 0.25R + 0.5G + 0.1094B
+ * @RKISP1_CIF_ISP_EXP_MEASURING_MODE_1: Y = (R + G + B) x (85/256)
+ */
+enum rkisp1_cif_isp_exp_meas_mode {
+ RKISP1_CIF_ISP_EXP_MEASURING_MODE_0,
+ RKISP1_CIF_ISP_EXP_MEASURING_MODE_1,
+};
+
+/*---------- PART1: Input Parameters ------------*/
+
+/**
+ * struct rkisp1_cif_isp_window - measurement window.
+ *
+ * Measurements are calculated per window inside the frame.
+ * This struct represents a window for a measurement.
+ *
+ * @h_offs: the horizontal offset of the window from the left of the frame in pixels.
+ * @v_offs: the vertical offset of the window from the top of the frame in pixels.
+ * @h_size: the horizontal size of the window in pixels
+ * @v_size: the vertical size of the window in pixels.
+ */
+struct rkisp1_cif_isp_window {
+ __u16 h_offs;
+ __u16 v_offs;
+ __u16 h_size;
+ __u16 v_size;
+};
+
+/**
+ * struct rkisp1_cif_isp_bls_fixed_val - BLS fixed subtraction values
+ *
+ * The values will be subtracted from the sensor
+ * values. Therefore a negative value means addition instead of subtraction!
+ *
+ * @r: Fixed (signed!) subtraction value for Bayer pattern R
+ * @gr: Fixed (signed!) subtraction value for Bayer pattern Gr
+ * @gb: Fixed (signed!) subtraction value for Bayer pattern Gb
+ * @b: Fixed (signed!) subtraction value for Bayer pattern B
+ */
+struct rkisp1_cif_isp_bls_fixed_val {
+ __s16 r;
+ __s16 gr;
+ __s16 gb;
+ __s16 b;
+};
+
+/**
+ * struct rkisp1_cif_isp_bls_config - Configuration used by black level subtraction
+ *
+ * @enable_auto: Automatic mode activated means that the measured values
+ * are subtracted. Otherwise the fixed subtraction
+ * values will be subtracted.
+ * @en_windows: enabled window
+ * @bls_window1: Measurement window 1 size
+ * @bls_window2: Measurement window 2 size
+ * @bls_samples: Set amount of measured pixels for each Bayer position
+ * (A, B,C and D) to 2^bls_samples.
+ * @fixed_val: Fixed subtraction values
+ */
+struct rkisp1_cif_isp_bls_config {
+ __u8 enable_auto;
+ __u8 en_windows;
+ struct rkisp1_cif_isp_window bls_window1;
+ struct rkisp1_cif_isp_window bls_window2;
+ __u8 bls_samples;
+ struct rkisp1_cif_isp_bls_fixed_val fixed_val;
+};
+
+/**
+ * struct rkisp1_cif_isp_dpcc_methods_config - Methods Configuration used by DPCC
+ *
+ * Methods Configuration used by Defect Pixel Cluster Correction
+ *
+ * @method: Method enable bits
+ * @line_thresh: Line threshold
+ * @line_mad_fac: Line MAD factor
+ * @pg_fac: Peak gradient factor
+ * @rnd_thresh: Rank Neighbor Difference threshold
+ * @rg_fac: Rank gradient factor
+ */
+struct rkisp1_cif_isp_dpcc_methods_config {
+ __u32 method;
+ __u32 line_thresh;
+ __u32 line_mad_fac;
+ __u32 pg_fac;
+ __u32 rnd_thresh;
+ __u32 rg_fac;
+};
+
+/**
+ * struct rkisp1_cif_isp_dpcc_config - Configuration used by DPCC
+ *
+ * Configuration used by Defect Pixel Cluster Correction
+ *
+ * @mode: dpcc output mode
+ * @output_mode: whether use hard coded methods
+ * @set_use: stage1 methods set
+ * @methods: methods config
+ * @ro_limits: rank order limits
+ * @rnd_offs: differential rank offsets for rank neighbor difference
+ */
+struct rkisp1_cif_isp_dpcc_config {
+ __u32 mode;
+ __u32 output_mode;
+ __u32 set_use;
+ struct rkisp1_cif_isp_dpcc_methods_config methods[RKISP1_CIF_ISP_DPCC_METHODS_MAX];
+ __u32 ro_limits;
+ __u32 rnd_offs;
+};
+
+/**
+ * struct rkisp1_cif_isp_gamma_corr_curve - gamma curve point definition y-axis (output).
+ *
+ * The reset values define a linear curve which has the same effect as bypass. Reset values are:
+ * gamma_y[0] = 0x0000, gamma_y[1] = 0x0100, ... gamma_y[15] = 0x0f00, gamma_y[16] = 0xfff
+ *
+ * @gamma_y: the values for the y-axis of gamma curve points. Each value is 12 bit.
+ */
+struct rkisp1_cif_isp_gamma_corr_curve {
+ __u16 gamma_y[RKISP1_CIF_ISP_DEGAMMA_CURVE_SIZE];
+};
+
+/**
+ * struct rkisp1_cif_isp_gamma_curve_x_axis_pnts - De-Gamma Curve definition x increments
+ * (sampling points). gamma_dx0 is for the lower samples (1-8), gamma_dx1 is for the
+ * higher samples (9-16). The reset values for both fields is 0x44444444. This means
+ * that each sample is 4 units away from the previous one on the x-axis.
+ *
+ * @gamma_dx0: gamma curve sample points definitions. Bits 0:2 for sample 1. Bit 3 unused.
+ * Bits 4:6 for sample 2. bit 7 unused ... Bits 28:30 for sample 8. Bit 31 unused
+ * @gamma_dx1: gamma curve sample points definitions. Bits 0:2 for sample 9. Bit 3 unused.
+ * Bits 4:6 for sample 10. bit 7 unused ... Bits 28:30 for sample 16. Bit 31 unused
+ */
+struct rkisp1_cif_isp_gamma_curve_x_axis_pnts {
+ __u32 gamma_dx0;
+ __u32 gamma_dx1;
+};
+
+/**
+ * struct rkisp1_cif_isp_sdg_config - Configuration used by sensor degamma
+ *
+ * @curve_r: gamma curve point definition axis for red
+ * @curve_g: gamma curve point definition axis for green
+ * @curve_b: gamma curve point definition axis for blue
+ * @xa_pnts: x axis increments
+ */
+struct rkisp1_cif_isp_sdg_config {
+ struct rkisp1_cif_isp_gamma_corr_curve curve_r;
+ struct rkisp1_cif_isp_gamma_corr_curve curve_g;
+ struct rkisp1_cif_isp_gamma_corr_curve curve_b;
+ struct rkisp1_cif_isp_gamma_curve_x_axis_pnts xa_pnts;
+};
+
+/**
+ * struct rkisp1_cif_isp_lsc_config - Configuration used by Lens shading correction
+ *
+ * @r_data_tbl: sample table red
+ * @gr_data_tbl: sample table green (red)
+ * @gb_data_tbl: sample table green (blue)
+ * @b_data_tbl: sample table blue
+ * @x_grad_tbl: gradient table x
+ * @y_grad_tbl: gradient table y
+ * @x_size_tbl: size table x
+ * @y_size_tbl: size table y
+ * @config_width: not used at the moment
+ * @config_height: not used at the moment
+ */
+struct rkisp1_cif_isp_lsc_config {
+ __u16 r_data_tbl[RKISP1_CIF_ISP_LSC_SAMPLES_MAX][RKISP1_CIF_ISP_LSC_SAMPLES_MAX];
+ __u16 gr_data_tbl[RKISP1_CIF_ISP_LSC_SAMPLES_MAX][RKISP1_CIF_ISP_LSC_SAMPLES_MAX];
+ __u16 gb_data_tbl[RKISP1_CIF_ISP_LSC_SAMPLES_MAX][RKISP1_CIF_ISP_LSC_SAMPLES_MAX];
+ __u16 b_data_tbl[RKISP1_CIF_ISP_LSC_SAMPLES_MAX][RKISP1_CIF_ISP_LSC_SAMPLES_MAX];
+
+ __u16 x_grad_tbl[RKISP1_CIF_ISP_LSC_SECTORS_TBL_SIZE];
+ __u16 y_grad_tbl[RKISP1_CIF_ISP_LSC_SECTORS_TBL_SIZE];
+
+ __u16 x_size_tbl[RKISP1_CIF_ISP_LSC_SECTORS_TBL_SIZE];
+ __u16 y_size_tbl[RKISP1_CIF_ISP_LSC_SECTORS_TBL_SIZE];
+ __u16 config_width;
+ __u16 config_height;
+};
+
+/**
+ * struct rkisp1_cif_isp_ie_config - Configuration used by image effects
+ *
+ * @effect: values from 'enum v4l2_colorfx'. Possible values are: V4L2_COLORFX_SEPIA,
+ * V4L2_COLORFX_SET_CBCR, V4L2_COLORFX_AQUA, V4L2_COLORFX_EMBOSS,
+ * V4L2_COLORFX_SKETCH, V4L2_COLORFX_BW, V4L2_COLORFX_NEGATIVE
+ * @color_sel: bits 0:2 - colors bitmask (001 - blue, 010 - green, 100 - red).
+ * bits 8:15 - Threshold value of the RGB colors for the color selection effect.
+ * @eff_mat_1: 3x3 Matrix Coefficients for Emboss Effect 1
+ * @eff_mat_2: 3x3 Matrix Coefficients for Emboss Effect 2
+ * @eff_mat_3: 3x3 Matrix Coefficients for Emboss 3/Sketch 1
+ * @eff_mat_4: 3x3 Matrix Coefficients for Sketch Effect 2
+ * @eff_mat_5: 3x3 Matrix Coefficients for Sketch Effect 3
+ * @eff_tint: Chrominance increment values of tint (used for sepia effect)
+ */
+struct rkisp1_cif_isp_ie_config {
+ __u16 effect;
+ __u16 color_sel;
+ __u16 eff_mat_1;
+ __u16 eff_mat_2;
+ __u16 eff_mat_3;
+ __u16 eff_mat_4;
+ __u16 eff_mat_5;
+ __u16 eff_tint;
+};
+
+/**
+ * struct rkisp1_cif_isp_cproc_config - Configuration used by Color Processing
+ *
+ * @c_out_range: Chrominance pixel clipping range at output.
+ * (0 for limit, 1 for full)
+ * @y_in_range: Luminance pixel clipping range at output.
+ * @y_out_range: Luminance pixel clipping range at output.
+ * @contrast: 00~ff, 0.0~1.992
+ * @brightness: 80~7F, -128~+127
+ * @sat: saturation, 00~FF, 0.0~1.992
+ * @hue: 80~7F, -90~+87.188
+ */
+struct rkisp1_cif_isp_cproc_config {
+ __u8 c_out_range;
+ __u8 y_in_range;
+ __u8 y_out_range;
+ __u8 contrast;
+ __u8 brightness;
+ __u8 sat;
+ __u8 hue;
+};
+
+/**
+ * struct rkisp1_cif_isp_awb_meas_config - Configuration for the AWB statistics
+ *
+ * @awb_mode: the awb meas mode. From enum rkisp1_cif_isp_awb_mode_type.
+ * @awb_wnd: white balance measurement window (in pixels)
+ * @max_y: only pixels values < max_y contribute to awb measurement, set to 0
+ * to disable this feature
+ * @min_y: only pixels values > min_y contribute to awb measurement
+ * @max_csum: Chrominance sum maximum value, only consider pixels with Cb+Cr,
+ * smaller than threshold for awb measurements
+ * @min_c: Chrominance minimum value, only consider pixels with Cb/Cr
+ * each greater than threshold value for awb measurements
+ * @frames: number of frames - 1 used for mean value calculation
+ * (ucFrames=0 means 1 Frame)
+ * @awb_ref_cr: reference Cr value for AWB regulation, target for AWB
+ * @awb_ref_cb: reference Cb value for AWB regulation, target for AWB
+ * @enable_ymax_cmp: enable Y_MAX compare (Not valid in RGB measurement mode.)
+ */
+struct rkisp1_cif_isp_awb_meas_config {
+ /*
+ * Note: currently the h and v offsets are mapped to grid offsets
+ */
+ struct rkisp1_cif_isp_window awb_wnd;
+ __u32 awb_mode;
+ __u8 max_y;
+ __u8 min_y;
+ __u8 max_csum;
+ __u8 min_c;
+ __u8 frames;
+ __u8 awb_ref_cr;
+ __u8 awb_ref_cb;
+ __u8 enable_ymax_cmp;
+};
+
+/**
+ * struct rkisp1_cif_isp_awb_gain_config - Configuration used by auto white balance gain
+ *
+ * All fields in this struct are 10 bit, where:
+ * 0x100h = 1, unsigned integer value, range 0 to 4 with 8 bit fractional part.
+ *
+ * out_data_x = ( AWB_GAIN_X * in_data + 128) >> 8
+ *
+ * @gain_red: gain value for red component.
+ * @gain_green_r: gain value for green component in red line.
+ * @gain_blue: gain value for blue component.
+ * @gain_green_b: gain value for green component in blue line.
+ */
+struct rkisp1_cif_isp_awb_gain_config {
+ __u16 gain_red;
+ __u16 gain_green_r;
+ __u16 gain_blue;
+ __u16 gain_green_b;
+};
+
+/**
+ * struct rkisp1_cif_isp_flt_config - Configuration used by ISP filtering
+ *
+ * All 4 threshold fields (thresh_*) are 10 bits.
+ * All 6 factor fields (fac_*) are 6 bits.
+ *
+ * @mode: ISP_FILT_MODE register fields (from enum rkisp1_cif_isp_flt_mode)
+ * @grn_stage1: Green filter stage 1 select (range 0x0...0x8)
+ * @chr_h_mode: Chroma filter horizontal mode
+ * @chr_v_mode: Chroma filter vertical mode
+ * @thresh_bl0: If thresh_bl1 < sum_grad < thresh_bl0 then fac_bl0 is selected (blurring th)
+ * @thresh_bl1: If sum_grad < thresh_bl1 then fac_bl1 is selected (blurring th)
+ * @thresh_sh0: If thresh_sh0 < sum_grad < thresh_sh1 then thresh_sh0 is selected (sharpening th)
+ * @thresh_sh1: If thresh_sh1 < sum_grad then thresh_sh1 is selected (sharpening th)
+ * @lum_weight: Parameters for luminance weight function.
+ * @fac_sh1: filter factor for sharp1 level
+ * @fac_sh0: filter factor for sharp0 level
+ * @fac_mid: filter factor for mid level and for static filter mode
+ * @fac_bl0: filter factor for blur 0 level
+ * @fac_bl1: filter factor for blur 1 level (max blur)
+ */
+struct rkisp1_cif_isp_flt_config {
+ __u32 mode;
+ __u8 grn_stage1;
+ __u8 chr_h_mode;
+ __u8 chr_v_mode;
+ __u32 thresh_bl0;
+ __u32 thresh_bl1;
+ __u32 thresh_sh0;
+ __u32 thresh_sh1;
+ __u32 lum_weight;
+ __u32 fac_sh1;
+ __u32 fac_sh0;
+ __u32 fac_mid;
+ __u32 fac_bl0;
+ __u32 fac_bl1;
+};
+
+/**
+ * struct rkisp1_cif_isp_bdm_config - Configuration used by Bayer DeMosaic
+ *
+ * @demosaic_th: threshold for bayer demosaicing texture detection
+ */
+struct rkisp1_cif_isp_bdm_config {
+ __u8 demosaic_th;
+};
+
+/**
+ * struct rkisp1_cif_isp_ctk_config - Configuration used by Cross Talk correction
+ *
+ * @coeff: color correction matrix. Values are 11-bit signed fixed-point numbers with 4 bit integer
+ * and 7 bit fractional part, ranging from -8 (0x400) to +7.992 (0x3FF). 0 is
+ * represented by 0x000 and a coefficient value of 1 as 0x080.
+ * @ct_offset: Red, Green, Blue offsets for the crosstalk correction matrix
+ */
+struct rkisp1_cif_isp_ctk_config {
+ __u16 coeff[3][3];
+ __u16 ct_offset[3];
+};
+
+enum rkisp1_cif_isp_goc_mode {
+ RKISP1_CIF_ISP_GOC_MODE_LOGARITHMIC,
+ RKISP1_CIF_ISP_GOC_MODE_EQUIDISTANT
+};
+
+/**
+ * struct rkisp1_cif_isp_goc_config - Configuration used by Gamma Out correction
+ *
+ * @mode: goc mode (from enum rkisp1_cif_isp_goc_mode)
+ * @gamma_y: gamma out curve y-axis for all color components
+ *
+ * The number of entries of @gamma_y depends on the hardware revision
+ * as is reported by the hw_revision field of the struct media_device_info
+ * that is returned by ioctl MEDIA_IOC_DEVICE_INFO.
+ *
+ * Versions <= V11 have RKISP1_CIF_ISP_GAMMA_OUT_MAX_SAMPLES_V10
+ * entries, versions >= V12 have RKISP1_CIF_ISP_GAMMA_OUT_MAX_SAMPLES_V12
+ * entries. RKISP1_CIF_ISP_GAMMA_OUT_MAX_SAMPLES is equal to the maximum
+ * of the two.
+ */
+struct rkisp1_cif_isp_goc_config {
+ __u32 mode;
+ __u16 gamma_y[RKISP1_CIF_ISP_GAMMA_OUT_MAX_SAMPLES];
+};
+
+/**
+ * struct rkisp1_cif_isp_hst_config - Configuration for Histogram statistics
+ *
+ * @mode: histogram mode (from enum rkisp1_cif_isp_histogram_mode)
+ * @histogram_predivider: process every stepsize pixel, all other pixels are
+ * skipped
+ * @meas_window: coordinates of the measure window
+ * @hist_weight: weighting factor for sub-windows
+ *
+ * The number of entries of @hist_weight depends on the hardware revision
+ * as is reported by the hw_revision field of the struct media_device_info
+ * that is returned by ioctl MEDIA_IOC_DEVICE_INFO.
+ *
+ * Versions <= V11 have RKISP1_CIF_ISP_HISTOGRAM_WEIGHT_GRIDS_SIZE_V10
+ * entries, versions >= V12 have RKISP1_CIF_ISP_HISTOGRAM_WEIGHT_GRIDS_SIZE_V12
+ * entries. RKISP1_CIF_ISP_HISTOGRAM_WEIGHT_GRIDS_SIZE is equal to the maximum
+ * of the two.
+ */
+struct rkisp1_cif_isp_hst_config {
+ __u32 mode;
+ __u8 histogram_predivider;
+ struct rkisp1_cif_isp_window meas_window;
+ __u8 hist_weight[RKISP1_CIF_ISP_HISTOGRAM_WEIGHT_GRIDS_SIZE];
+};
+
+/**
+ * struct rkisp1_cif_isp_aec_config - Configuration for Auto Exposure statistics
+ *
+ * @mode: Exposure measure mode (from enum rkisp1_cif_isp_exp_meas_mode)
+ * @autostop: stop mode (from enum rkisp1_cif_isp_exp_ctrl_autostop)
+ * @meas_window: coordinates of the measure window
+ */
+struct rkisp1_cif_isp_aec_config {
+ __u32 mode;
+ __u32 autostop;
+ struct rkisp1_cif_isp_window meas_window;
+};
+
+/**
+ * struct rkisp1_cif_isp_afc_config - Configuration for the Auto Focus statistics
+ *
+ * @num_afm_win: max RKISP1_CIF_ISP_AFM_MAX_WINDOWS
+ * @afm_win: coordinates of the meas window
+ * @thres: threshold used for minimizing the influence of noise
+ * @var_shift: the number of bits for the shift operation at the end of the
+ * calculation chain.
+ */
+struct rkisp1_cif_isp_afc_config {
+ __u8 num_afm_win;
+ struct rkisp1_cif_isp_window afm_win[RKISP1_CIF_ISP_AFM_MAX_WINDOWS];
+ __u32 thres;
+ __u32 var_shift;
+};
+
+/**
+ * enum rkisp1_cif_isp_dpf_gain_usage - dpf gain usage
+ * @RKISP1_CIF_ISP_DPF_GAIN_USAGE_DISABLED: don't use any gains in preprocessing stage
+ * @RKISP1_CIF_ISP_DPF_GAIN_USAGE_NF_GAINS: use only the noise function gains from
+ * registers DPF_NF_GAIN_R, ...
+ * @RKISP1_CIF_ISP_DPF_GAIN_USAGE_LSC_GAINS: use only the gains from LSC module
+ * @RKISP1_CIF_ISP_DPF_GAIN_USAGE_NF_LSC_GAINS: use the noise function gains and the
+ * gains from LSC module
+ * @RKISP1_CIF_ISP_DPF_GAIN_USAGE_AWB_GAINS: use only the gains from AWB module
+ * @RKISP1_CIF_ISP_DPF_GAIN_USAGE_AWB_LSC_GAINS: use the gains from AWB and LSC module
+ * @RKISP1_CIF_ISP_DPF_GAIN_USAGE_MAX: upper border (only for an internal evaluation)
+ */
+enum rkisp1_cif_isp_dpf_gain_usage {
+ RKISP1_CIF_ISP_DPF_GAIN_USAGE_DISABLED,
+ RKISP1_CIF_ISP_DPF_GAIN_USAGE_NF_GAINS,
+ RKISP1_CIF_ISP_DPF_GAIN_USAGE_LSC_GAINS,
+ RKISP1_CIF_ISP_DPF_GAIN_USAGE_NF_LSC_GAINS,
+ RKISP1_CIF_ISP_DPF_GAIN_USAGE_AWB_GAINS,
+ RKISP1_CIF_ISP_DPF_GAIN_USAGE_AWB_LSC_GAINS,
+ RKISP1_CIF_ISP_DPF_GAIN_USAGE_MAX
+};
+
+/**
+ * enum rkisp1_cif_isp_dpf_rb_filtersize - Red and blue filter sizes
+ * @RKISP1_CIF_ISP_DPF_RB_FILTERSIZE_13x9: red and blue filter kernel size 13x9
+ * (means 7x5 active pixel)
+ * @RKISP1_CIF_ISP_DPF_RB_FILTERSIZE_9x9: red and blue filter kernel size 9x9
+ * (means 5x5 active pixel)
+ */
+enum rkisp1_cif_isp_dpf_rb_filtersize {
+ RKISP1_CIF_ISP_DPF_RB_FILTERSIZE_13x9,
+ RKISP1_CIF_ISP_DPF_RB_FILTERSIZE_9x9,
+};
+
+/**
+ * enum rkisp1_cif_isp_dpf_nll_scale_mode - dpf noise level scale mode
+ * @RKISP1_CIF_ISP_NLL_SCALE_LINEAR: use a linear scaling
+ * @RKISP1_CIF_ISP_NLL_SCALE_LOGARITHMIC: use a logarithmic scaling
+ */
+enum rkisp1_cif_isp_dpf_nll_scale_mode {
+ RKISP1_CIF_ISP_NLL_SCALE_LINEAR,
+ RKISP1_CIF_ISP_NLL_SCALE_LOGARITHMIC,
+};
+
+/**
+ * struct rkisp1_cif_isp_dpf_nll - Noise level lookup
+ *
+ * @coeff: Noise level Lookup coefficient
+ * @scale_mode: dpf noise level scale mode (from enum rkisp1_cif_isp_dpf_nll_scale_mode)
+ */
+struct rkisp1_cif_isp_dpf_nll {
+ __u16 coeff[RKISP1_CIF_ISP_DPF_MAX_NLF_COEFFS];
+ __u32 scale_mode;
+};
+
+/**
+ * struct rkisp1_cif_isp_dpf_rb_flt - Red blue filter config
+ *
+ * @fltsize: The filter size for the red and blue pixels
+ * (from enum rkisp1_cif_isp_dpf_rb_filtersize)
+ * @spatial_coeff: Spatial weights
+ * @r_enable: enable filter processing for red pixels
+ * @b_enable: enable filter processing for blue pixels
+ */
+struct rkisp1_cif_isp_dpf_rb_flt {
+ __u32 fltsize;
+ __u8 spatial_coeff[RKISP1_CIF_ISP_DPF_MAX_SPATIAL_COEFFS];
+ __u8 r_enable;
+ __u8 b_enable;
+};
+
+/**
+ * struct rkisp1_cif_isp_dpf_g_flt - Green filter Configuration
+ *
+ * @spatial_coeff: Spatial weights
+ * @gr_enable: enable filter processing for green pixels in green/red lines
+ * @gb_enable: enable filter processing for green pixels in green/blue lines
+ */
+struct rkisp1_cif_isp_dpf_g_flt {
+ __u8 spatial_coeff[RKISP1_CIF_ISP_DPF_MAX_SPATIAL_COEFFS];
+ __u8 gr_enable;
+ __u8 gb_enable;
+};
+
+/**
+ * struct rkisp1_cif_isp_dpf_gain - Noise function Configuration
+ *
+ * @mode: dpf gain usage (from enum rkisp1_cif_isp_dpf_gain_usage)
+ * @nf_r_gain: Noise function Gain that replaces the AWB gain for red pixels
+ * @nf_b_gain: Noise function Gain that replaces the AWB gain for blue pixels
+ * @nf_gr_gain: Noise function Gain that replaces the AWB gain
+ * for green pixels in a red line
+ * @nf_gb_gain: Noise function Gain that replaces the AWB gain
+ * for green pixels in a blue line
+ */
+struct rkisp1_cif_isp_dpf_gain {
+ __u32 mode;
+ __u16 nf_r_gain;
+ __u16 nf_b_gain;
+ __u16 nf_gr_gain;
+ __u16 nf_gb_gain;
+};
+
+/**
+ * struct rkisp1_cif_isp_dpf_config - Configuration used by De-noising pre-filter
+ *
+ * @gain: noise function gain
+ * @g_flt: green filter config
+ * @rb_flt: red blue filter config
+ * @nll: noise level lookup
+ */
+struct rkisp1_cif_isp_dpf_config {
+ struct rkisp1_cif_isp_dpf_gain gain;
+ struct rkisp1_cif_isp_dpf_g_flt g_flt;
+ struct rkisp1_cif_isp_dpf_rb_flt rb_flt;
+ struct rkisp1_cif_isp_dpf_nll nll;
+};
+
+/**
+ * struct rkisp1_cif_isp_dpf_strength_config - strength of the filter
+ *
+ * @r: filter strength of the RED filter
+ * @g: filter strength of the GREEN filter
+ * @b: filter strength of the BLUE filter
+ */
+struct rkisp1_cif_isp_dpf_strength_config {
+ __u8 r;
+ __u8 g;
+ __u8 b;
+};
+
+/**
+ * struct rkisp1_cif_isp_isp_other_cfg - Parameters for some blocks in rockchip isp1
+ *
+ * @dpcc_config: Defect Pixel Cluster Correction config
+ * @bls_config: Black Level Subtraction config
+ * @sdg_config: sensor degamma config
+ * @lsc_config: Lens Shade config
+ * @awb_gain_config: Auto White balance gain config
+ * @flt_config: filter config
+ * @bdm_config: demosaic config
+ * @ctk_config: cross talk config
+ * @goc_config: gamma out config
+ * @bls_config: black level subtraction config
+ * @dpf_config: De-noising pre-filter config
+ * @dpf_strength_config: dpf strength config
+ * @cproc_config: color process config
+ * @ie_config: image effects config
+ */
+struct rkisp1_cif_isp_isp_other_cfg {
+ struct rkisp1_cif_isp_dpcc_config dpcc_config;
+ struct rkisp1_cif_isp_bls_config bls_config;
+ struct rkisp1_cif_isp_sdg_config sdg_config;
+ struct rkisp1_cif_isp_lsc_config lsc_config;
+ struct rkisp1_cif_isp_awb_gain_config awb_gain_config;
+ struct rkisp1_cif_isp_flt_config flt_config;
+ struct rkisp1_cif_isp_bdm_config bdm_config;
+ struct rkisp1_cif_isp_ctk_config ctk_config;
+ struct rkisp1_cif_isp_goc_config goc_config;
+ struct rkisp1_cif_isp_dpf_config dpf_config;
+ struct rkisp1_cif_isp_dpf_strength_config dpf_strength_config;
+ struct rkisp1_cif_isp_cproc_config cproc_config;
+ struct rkisp1_cif_isp_ie_config ie_config;
+};
+
+/**
+ * struct rkisp1_cif_isp_isp_meas_cfg - Rockchip ISP1 Measure Parameters
+ *
+ * @awb_meas_config: auto white balance config
+ * @hst_config: histogram config
+ * @aec_config: auto exposure config
+ * @afc_config: auto focus config
+ */
+struct rkisp1_cif_isp_isp_meas_cfg {
+ struct rkisp1_cif_isp_awb_meas_config awb_meas_config;
+ struct rkisp1_cif_isp_hst_config hst_config;
+ struct rkisp1_cif_isp_aec_config aec_config;
+ struct rkisp1_cif_isp_afc_config afc_config;
+};
+
+/**
+ * struct rkisp1_params_cfg - Rockchip ISP1 Input Parameters Meta Data
+ *
+ * @module_en_update: mask the enable bits of which module should be updated
+ * @module_ens: mask the enable value of each module, only update the module
+ * which correspond bit was set in module_en_update
+ * @module_cfg_update: mask the config bits of which module should be updated
+ * @meas: measurement config
+ * @others: other config
+ */
+struct rkisp1_params_cfg {
+ __u32 module_en_update;
+ __u32 module_ens;
+ __u32 module_cfg_update;
+
+ struct rkisp1_cif_isp_isp_meas_cfg meas;
+ struct rkisp1_cif_isp_isp_other_cfg others;
+};
+
+/*---------- PART2: Measurement Statistics ------------*/
+
+/**
+ * struct rkisp1_cif_isp_awb_meas - AWB measured values
+ *
+ * @cnt: White pixel count, number of "white pixels" found during last
+ * measurement
+ * @mean_y_or_g: Mean value of Y within window and frames,
+ * Green if RGB is selected.
+ * @mean_cb_or_b: Mean value of Cb within window and frames,
+ * Blue if RGB is selected.
+ * @mean_cr_or_r: Mean value of Cr within window and frames,
+ * Red if RGB is selected.
+ */
+struct rkisp1_cif_isp_awb_meas {
+ __u32 cnt;
+ __u8 mean_y_or_g;
+ __u8 mean_cb_or_b;
+ __u8 mean_cr_or_r;
+};
+
+/**
+ * struct rkisp1_cif_isp_awb_stat - statistics automatic white balance data
+ *
+ * @awb_mean: Mean measured data
+ */
+struct rkisp1_cif_isp_awb_stat {
+ struct rkisp1_cif_isp_awb_meas awb_mean[RKISP1_CIF_ISP_AWB_MAX_GRID];
+};
+
+/**
+ * struct rkisp1_cif_isp_bls_meas_val - BLS measured values
+ *
+ * @meas_r: Mean measured value for Bayer pattern R
+ * @meas_gr: Mean measured value for Bayer pattern Gr
+ * @meas_gb: Mean measured value for Bayer pattern Gb
+ * @meas_b: Mean measured value for Bayer pattern B
+ */
+struct rkisp1_cif_isp_bls_meas_val {
+ __u16 meas_r;
+ __u16 meas_gr;
+ __u16 meas_gb;
+ __u16 meas_b;
+};
+
+/**
+ * struct rkisp1_cif_isp_ae_stat - statistics auto exposure data
+ *
+ * @exp_mean: Mean luminance value of block xx
+ * @bls_val: BLS measured values
+ *
+ * The number of entries of @exp_mean depends on the hardware revision
+ * as is reported by the hw_revision field of the struct media_device_info
+ * that is returned by ioctl MEDIA_IOC_DEVICE_INFO.
+ *
+ * Versions <= V11 have RKISP1_CIF_ISP_AE_MEAN_MAX_V10 entries,
+ * versions >= V12 have RKISP1_CIF_ISP_AE_MEAN_MAX_V12 entries.
+ * RKISP1_CIF_ISP_AE_MEAN_MAX is equal to the maximum of the two.
+ *
+ * Image is divided into 5x5 blocks on V10 and 9x9 blocks on V12.
+ */
+struct rkisp1_cif_isp_ae_stat {
+ __u8 exp_mean[RKISP1_CIF_ISP_AE_MEAN_MAX];
+ struct rkisp1_cif_isp_bls_meas_val bls_val;
+};
+
+/**
+ * struct rkisp1_cif_isp_af_meas_val - AF measured values
+ *
+ * @sum: sharpness value
+ * @lum: luminance value
+ */
+struct rkisp1_cif_isp_af_meas_val {
+ __u32 sum;
+ __u32 lum;
+};
+
+/**
+ * struct rkisp1_cif_isp_af_stat - statistics auto focus data
+ *
+ * @window: AF measured value of window x
+ *
+ * The module measures the sharpness in 3 windows of selectable size via
+ * register settings(ISP_AFM_*_A/B/C)
+ */
+struct rkisp1_cif_isp_af_stat {
+ struct rkisp1_cif_isp_af_meas_val window[RKISP1_CIF_ISP_AFM_MAX_WINDOWS];
+};
+
+/**
+ * struct rkisp1_cif_isp_hist_stat - statistics histogram data
+ *
+ * @hist_bins: measured bin counters. Each bin is a 20 bits unsigned fixed point
+ * type. Bits 0-4 are the fractional part and bits 5-19 are the
+ * integer part.
+ *
+ * The window of the measurements area is divided to 5x5 sub-windows for
+ * V10/V11 and to 9x9 sub-windows for V12. The histogram is then computed for
+ * each sub-window independently and the final result is a weighted average of
+ * the histogram measurements on all sub-windows. The window of the
+ * measurements area and the weight of each sub-window are configurable using
+ * struct @rkisp1_cif_isp_hst_config.
+ *
+ * The histogram contains 16 bins in V10/V11 and 32 bins in V12/V13.
+ *
+ * The number of entries of @hist_bins depends on the hardware revision
+ * as is reported by the hw_revision field of the struct media_device_info
+ * that is returned by ioctl MEDIA_IOC_DEVICE_INFO.
+ *
+ * Versions <= V11 have RKISP1_CIF_ISP_HIST_BIN_N_MAX_V10 entries,
+ * versions >= V12 have RKISP1_CIF_ISP_HIST_BIN_N_MAX_V12 entries.
+ * RKISP1_CIF_ISP_HIST_BIN_N_MAX is equal to the maximum of the two.
+ */
+struct rkisp1_cif_isp_hist_stat {
+ __u32 hist_bins[RKISP1_CIF_ISP_HIST_BIN_N_MAX];
+};
+
+/**
+ * struct rkisp1_cif_isp_stat - Rockchip ISP1 Statistics Data
+ *
+ * @awb: statistics data for automatic white balance
+ * @ae: statistics data for auto exposure
+ * @af: statistics data for auto focus
+ * @hist: statistics histogram data
+ */
+struct rkisp1_cif_isp_stat {
+ struct rkisp1_cif_isp_awb_stat awb;
+ struct rkisp1_cif_isp_ae_stat ae;
+ struct rkisp1_cif_isp_af_stat af;
+ struct rkisp1_cif_isp_hist_stat hist;
+};
+
+/**
+ * struct rkisp1_stat_buffer - Rockchip ISP1 Statistics Meta Data
+ *
+ * @meas_type: measurement types (RKISP1_CIF_ISP_STAT_* definitions)
+ * @frame_id: frame ID for sync
+ * @params: statistics data
+ */
+struct rkisp1_stat_buffer {
+ __u32 meas_type;
+ __u32 frame_id;
+ struct rkisp1_cif_isp_stat params;
+};
+
+#endif /* _UAPI_RKISP1_CONFIG_H */
diff --git a/y2023/vision/target_map.json b/y2023/vision/target_map.json
new file mode 100644
index 0000000..3f6eb54
--- /dev/null
+++ b/y2023/vision/target_map.json
@@ -0,0 +1,124 @@
+{
+ "target_poses": [
+ {
+ "id": 1,
+ "position": {
+ "x": 7.244,
+ "y": -2.938,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
+ },
+ {
+ "id": 2,
+ "position": {
+ "x": 7.244,
+ "y": -1.262,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
+ },
+ {
+ "id": 3,
+ "position": {
+ "x": 7.244,
+ "y": 0.414,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
+ },
+ {
+ "id": 4,
+ "position": {
+ "x": 7.909,
+ "y": 2.740,
+ "z": 0.695
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
+ },
+ {
+ "id": 5,
+ "position": {
+ "x": -7.908,
+ "y": 2.740,
+ "z": 0.695
+ },
+ "orientation": {
+ /* yaw of 0 */
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
+ },
+ {
+ "id": 6,
+ "position": {
+ "x": -7.243,
+ "y": 0.414,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
+ },
+ {
+ "id": 7,
+ "position": {
+ "x": -7.243,
+ "y": -1.262,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
+ },
+ {
+ "id": 8,
+ "position": {
+ "x": -7.243,
+ "y": -2.938,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
+ }
+ ]
+}
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
new file mode 100644
index 0000000..6d32dbd
--- /dev/null
+++ b/y2023/vision/target_mapping.cc
@@ -0,0 +1,205 @@
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/target_mapper.h"
+#include "opencv2/aruco.hpp"
+#include "opencv2/calib3d.hpp"
+#include "opencv2/core/eigen.hpp"
+#include "opencv2/features2d.hpp"
+#include "opencv2/highgui.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc.hpp"
+#include "y2023/vision/aprilrobotics.h"
+#include "y2023/vision/calibration_data.h"
+
+DEFINE_string(json_path, "target_map.json",
+ "Specify path for json with initial pose guesses.");
+DECLARE_int32(team_number);
+
+namespace y2023 {
+namespace vision {
+using frc971::vision::CharucoExtractor;
+using frc971::vision::DataAdapter;
+using frc971::vision::ImageCallback;
+using frc971::vision::PoseUtils;
+using frc971::vision::TargetMap;
+using frc971::vision::TargetMapper;
+namespace calibration = frc971::vision::calibration;
+
+// Change reference frame from camera to robot
+Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camrob_target,
+ Eigen::Affine3d extrinsics) {
+ const Eigen::Affine3d H_robot_camrob = extrinsics;
+ const Eigen::Affine3d H_robot_target = H_robot_camrob * H_camrob_target;
+ return H_robot_target;
+}
+
+// Add detected apriltag poses relative to the robot to
+// timestamped_target_detections
+void HandleAprilTag(const TargetMap &map,
+ aos::distributed_clock::time_point pi_distributed_time,
+ std::vector<DataAdapter::TimestampedDetection>
+ *timestamped_target_detections,
+ Eigen::Affine3d extrinsics) {
+ for (const auto *target_pose_fbs : *map.target_poses()) {
+ const TargetMapper::TargetPose target_pose =
+ PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
+
+ Eigen::Affine3d H_camcv_target =
+ Eigen::Translation3d(target_pose.pose.p) * target_pose.pose.q;
+ // With X, Y, Z being robot axes and x, y, z being camera axes,
+ // x = -Y, y = -Z, z = X
+ static const Eigen::Affine3d H_camcv_camrob =
+ Eigen::Affine3d((Eigen::Matrix4d() << 0.0, -1.0, 0.0, 0.0, 0.0, 0.0,
+ -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)
+ .finished());
+ Eigen::Affine3d H_camrob_target = H_camcv_camrob.inverse() * H_camcv_target;
+ Eigen::Affine3d H_robot_target =
+ CameraToRobotDetection(H_camrob_target, extrinsics);
+
+ ceres::examples::Pose3d target_pose_camera =
+ PoseUtils::Affine3dToPose3d(H_camrob_target);
+ double distance_from_camera = target_pose_camera.p.norm();
+
+ CHECK(map.has_monotonic_timestamp_ns())
+ << "Need detection timestamps for mapping";
+
+ timestamped_target_detections->emplace_back(
+ DataAdapter::TimestampedDetection{
+ .time = pi_distributed_time,
+ .H_robot_target = H_robot_target,
+ .distance_from_camera = distance_from_camera,
+ .id = static_cast<TargetMapper::TargetId>(target_pose.id)});
+ }
+}
+
+const calibration::CameraCalibration *FindCameraCalibration(
+ const calibration::CalibrationData *calibration_data,
+ std::string_view node_name) {
+ for (const calibration::CameraCalibration *candidate :
+ *calibration_data->camera_calibrations()) {
+ if (candidate->node_name()->string_view() != node_name) {
+ continue;
+ }
+ if (candidate->team_number() != FLAGS_team_number) {
+ continue;
+ }
+ return candidate;
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+ << " on " << FLAGS_team_number;
+}
+
+Eigen::Affine3d CameraExtrinsics(
+ const calibration::CameraCalibration *camera_calibration) {
+ const frc971::vision::calibration::TransformationMatrix *transform =
+ camera_calibration->has_turret_extrinsics()
+ ? camera_calibration->turret_extrinsics()
+ : camera_calibration->fixed_extrinsics();
+
+ cv::Mat result(
+ 4, 4, CV_32F,
+ const_cast<void *>(static_cast<const void *>(transform->data()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(), transform->data()->size());
+
+ Eigen::Matrix4d result_eigen;
+ cv::cv2eigen(result, result_eigen);
+ return Eigen::Affine3d(result_eigen);
+}
+
+// Get images from pi and pass apriltag positions to HandleAprilTag()
+void HandlePiCaptures(
+ aos::EventLoop *pi_event_loop, aos::logger::LogReader *reader,
+ std::vector<DataAdapter::TimestampedDetection>
+ *timestamped_target_detections,
+ std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors) {
+ const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
+ CalibrationData());
+
+ const auto node_name = pi_event_loop->node()->name()->string_view();
+ const calibration::CameraCalibration *calibration =
+ FindCameraCalibration(&calibration_data.message(), node_name);
+ const auto extrinsics = CameraExtrinsics(calibration);
+
+ // TODO(milind): change to /camera once we log at full frequency
+ static constexpr std::string_view kImageChannel = "/camera/decimated";
+ detectors->emplace_back(
+ std::make_unique<AprilRoboticsDetector>(pi_event_loop, kImageChannel));
+
+ pi_event_loop->MakeWatcher("/camera", [=](const TargetMap &map) {
+ aos::distributed_clock::time_point pi_distributed_time =
+ reader->event_loop_factory()
+ ->GetNodeEventLoopFactory(pi_event_loop->node())
+ ->ToDistributedClock(aos::monotonic_clock::time_point(
+ aos::monotonic_clock::duration(map.monotonic_timestamp_ns())));
+
+ HandleAprilTag(map, pi_distributed_time, timestamped_target_detections,
+ extrinsics);
+ });
+}
+
+void MappingMain(int argc, char *argv[]) {
+ std::vector<std::string> unsorted_logfiles =
+ aos::logger::FindLogs(argc, argv);
+
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections;
+
+ // open logfiles
+ aos::logger::LogReader reader(aos::logger::SortParts(unsorted_logfiles));
+ reader.Register();
+
+ std::vector<std::unique_ptr<AprilRoboticsDetector>> detectors;
+
+ const aos::Node *pi1 =
+ aos::configuration::GetNode(reader.configuration(), "pi1");
+ std::unique_ptr<aos::EventLoop> pi1_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("pi1", pi1);
+ HandlePiCaptures(pi1_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
+
+ const aos::Node *pi2 =
+ aos::configuration::GetNode(reader.configuration(), "pi2");
+ std::unique_ptr<aos::EventLoop> pi2_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("pi2", pi2);
+ HandlePiCaptures(pi2_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
+
+ const aos::Node *pi3 =
+ aos::configuration::GetNode(reader.configuration(), "pi3");
+ std::unique_ptr<aos::EventLoop> pi3_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("pi3", pi3);
+ HandlePiCaptures(pi3_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
+
+ const aos::Node *pi4 =
+ aos::configuration::GetNode(reader.configuration(), "pi4");
+ std::unique_ptr<aos::EventLoop> pi4_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("pi4", pi4);
+ HandlePiCaptures(pi4_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
+
+ reader.event_loop_factory()->Run();
+
+ auto target_constraints =
+ DataAdapter::MatchTargetDetections(timestamped_target_detections);
+
+ frc971::vision::TargetMapper mapper(FLAGS_json_path, target_constraints);
+ mapper.Solve("charged_up");
+
+ // Clean up all the pointers
+ for (auto &detector_ptr : detectors) {
+ detector_ptr.reset();
+ }
+}
+
+} // namespace vision
+} // namespace y2023
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2023::vision::MappingMain(argc, argv);
+}
diff --git a/y2023/vision/viewer.cc b/y2023/vision/viewer.cc
index 990ad7a..68495b1 100644
--- a/y2023/vision/viewer.cc
+++ b/y2023/vision/viewer.cc
@@ -1,8 +1,10 @@
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
+#include "absl/strings/match.h"
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
#include "aos/time/time.h"
#include "frc971/vision/vision_generated.h"
@@ -12,6 +14,8 @@
DEFINE_string(capture, "",
"If set, capture a single image and save it to this filename.");
+DEFINE_int32(rate, 100, "Time in milliseconds to wait between images");
+
namespace frc971 {
namespace vision {
namespace {
@@ -36,7 +40,12 @@
cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
if (!FLAGS_capture.empty()) {
- cv::imwrite(FLAGS_capture, bgr_image);
+ if (absl::EndsWith(FLAGS_capture, ".bfbs")) {
+ aos::WriteFlatbufferToFile(FLAGS_capture, image_fetcher.CopyFlatBuffer());
+ } else {
+ cv::imwrite(FLAGS_capture, bgr_image);
+ }
+
return false;
}
@@ -71,7 +80,7 @@
event_loop.Exit();
};
},
- ::std::chrono::milliseconds(100));
+ ::std::chrono::milliseconds(FLAGS_rate));
event_loop.Run();
diff --git a/y2023/y2023.json b/y2023/y2023.json
index 76f0e52..d5f9462 100644
--- a/y2023/y2023.json
+++ b/y2023/y2023.json
@@ -1,5 +1,5 @@
{
- "channel_storage_duration": 2000000000,
+ "channel_storage_duration": 10000000000,
"maps": [
{
"match": {
diff --git a/y2023/y2023_imu.json b/y2023/y2023_imu.json
index cc02067..7744281 100644
--- a/y2023/y2023_imu.json
+++ b/y2023/y2023_imu.json
@@ -229,7 +229,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "logger",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
@@ -287,7 +287,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "roborio",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
diff --git a/y2023/y2023_logger.json b/y2023/y2023_logger.json
index 09038ae..db313d2 100644
--- a/y2023/y2023_logger.json
+++ b/y2023/y2023_logger.json
@@ -207,7 +207,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "logger",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
diff --git a/y2023/y2023_pi_template.json b/y2023/y2023_pi_template.json
index 1430753..fac37b2 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -164,12 +164,59 @@
"type": "frc971.vision.CameraImage",
"source_node": "pi{{ NUM }}",
"frequency": 40,
- "max_size": 2600000,
+ "max_size": 1843456,
"num_readers": 4,
"read_method": "PIN",
"num_senders": 18
},
{
+ "name": "/pi{{ NUM }}/camera",
+ "type": "frc971.vision.TargetMap",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 40,
+ "num_senders": 2,
+ "max_size": 40000,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu",
+ "logger"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 4,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "pi{{ NUM }}"
+ ],
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 4,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "pi{{ NUM }}"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/pi{{ NUM }}/aos/remote_timestamps/imu/pi{{ NUM }}/camera/frc971-vision-TargetMap",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 80,
+ "source_node": "pi{{ NUM }}",
+ "max_size": 208
+ },
+ {
+ "name": "/pi{{ NUM }}/aos/remote_timestamps/logger/pi{{ NUM }}/camera/frc971-vision-TargetMap",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 80,
+ "source_node": "pi{{ NUM }}",
+ "max_size": 208
+ },
+ {
"name": "/logger/aos",
"type": "aos.starter.StarterRpc",
"source_node": "logger",
@@ -215,7 +262,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "logger",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},
@@ -265,7 +312,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "roborio",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
}
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index 4c5b282..e1306c2 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -99,7 +99,7 @@
"type": "aos.message_bridge.RemoteMessage",
"source_node": "roborio",
"logger": "NOT_LOGGED",
- "frequency": 20,
+ "frequency": 50,
"num_senders": 2,
"max_size": 200
},