Merge "Add ssd profiler for tuning SSD write performance"
diff --git a/BUILD b/BUILD
index a568e20..413620d 100644
--- a/BUILD
+++ b/BUILD
@@ -35,6 +35,8 @@
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/refresh_match_list_response //scouting/webserver/requests/messages:refresh_match_list_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_shift_schedule //scouting/webserver/requests/messages:request_shift_schedule_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/request_shift_schedule_response //scouting/webserver/requests/messages:request_shift_schedule_response_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_actions //scouting/webserver/requests/messages:submit_actions_go_fbs
+# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_actions_response //scouting/webserver/requests/messages:submit_actions_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_shift_schedule //scouting/webserver/requests/messages:submit_shift_schedule_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_shift_schedule_response //scouting/webserver/requests/messages:submit_shift_schedule_response_go_fbs
# gazelle:resolve go github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_driver_ranking //scouting/webserver/requests/messages:submit_driver_ranking_go_fbs
diff --git a/WORKSPACE b/WORKSPACE
index 3e1801c..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",
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/events/event_loop.cc b/aos/events/event_loop.cc
index f5e9f51..c29d820 100644
--- a/aos/events/event_loop.cc
+++ b/aos/events/event_loop.cc
@@ -236,7 +236,8 @@
CHECK(taken_senders_.find(channel) == taken_senders_.end())
<< ": " << configuration::CleanedChannelToString(channel)
- << " is already being used.";
+ << " is already being used for sending. Can't make a watcher on the "
+ "same event loop.";
auto result = taken_watchers_.insert(channel);
CHECK(result.second) << ": " << configuration::CleanedChannelToString(channel)
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/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index ab56356..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;
}
diff --git a/aos/events/logging/logfile_utils.h b/aos/events/logging/logfile_utils.h
index 346a36e..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_; }
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 f453798..b3a9bbd 100644
--- a/aos/events/logging/logfile_utils_test.cc
+++ b/aos/events/logging/logfile_utils_test.cc
@@ -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.
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/util/BUILD b/aos/util/BUILD
index 48dc054..929b376 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -466,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/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..96375f0
--- /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();
+ VLOG(1) << "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) {
+ VLOG(1) << "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) {
+ VLOG(1) << "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/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/vision/BUILD b/frc971/vision/BUILD
index c52afae..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(
@@ -76,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",
],
@@ -102,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",
@@ -121,6 +124,7 @@
visibility = ["//visibility:public"],
deps = [
":charuco_lib",
+ ":foxglove_image_converter",
"//aos:init",
"//aos/events/logging:log_reader",
"//frc971/analysis:in_process_plotter",
@@ -129,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",
@@ -258,9 +264,42 @@
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/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 622c871..f12a6a5 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -480,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 7759482..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);
@@ -178,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/foxglove_image_converter.cc b/frc971/vision/foxglove_image_converter.cc
index 0c5c736..abde78b 100644
--- a/frc971/vision/foxglove_image_converter.cc
+++ b/frc971/vision/foxglove_image_converter.cc
@@ -3,7 +3,6 @@
#include <opencv2/imgproc.hpp>
namespace frc971::vision {
-namespace {
std::string_view ExtensionForCompression(ImageCompression compression) {
switch (compression) {
case ImageCompression::kJpeg:
@@ -12,25 +11,18 @@
return "png";
}
}
-} // namespace
+
flatbuffers::Offset<foxglove::CompressedImage> CompressImage(
- const CameraImage *raw_image, flatbuffers::FlatBufferBuilder *fbb,
- ImageCompression compression) {
+ 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(raw_image->has_data());
- cv::Mat image_color_mat(cv::Size(raw_image->cols(), raw_image->rows()),
- CV_8UC2, (void *)raw_image->data()->data());
- cv::Mat bgr_image(cv::Size(raw_image->cols(), raw_image->rows()), CV_8UC3);
- cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
- CHECK(cv::imencode(absl::StrCat(".", format), bgr_image, 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(aos::monotonic_clock::time_point(
- std::chrono::nanoseconds(raw_image->monotonic_timestamp_ns())));
+ 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 =
@@ -47,13 +39,14 @@
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)) {
- event_loop_->MakeWatcher(input_channel, [this, compression](
- const CameraImage &image) {
- auto builder = sender_.MakeBuilder();
- builder.CheckOk(
- builder.Send(CompressImage(&image, builder.fbb(), compression)));
- });
-}
+ 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
index add83a6..872ac14 100644
--- a/frc971/vision/foxglove_image_converter.h
+++ b/frc971/vision/foxglove_image_converter.h
@@ -1,8 +1,9 @@
#ifndef FRC971_VISION_FOXGLOVE_IMAGE_CONVERTER_H_
#define FRC971_VISION_FOXGLOVE_IMAGE_CONVERTER_H_
-#include "external/com_github_foxglove_schemas/CompressedImage_generated.h"
-#include "frc971/vision/vision_generated.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:
@@ -12,9 +13,11 @@
// 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 CameraImage *raw_image, flatbuffers::FlatBufferBuilder *fbb,
- ImageCompression compression);
+ 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
@@ -30,6 +33,7 @@
private:
aos::EventLoop *event_loop_;
+ ImageCallback image_callback_;
aos::Sender<foxglove::CompressedImage> sender_;
};
} // namespace frc971::vision
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/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
index a6bcb4d..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;
diff --git a/frc971/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
index 669c157..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);
@@ -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_;
@@ -159,11 +177,17 @@
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 85e3d98..8f043de 100644
--- a/scouting/db/db.go
+++ b/scouting/db/db.go
@@ -66,6 +66,34 @@
CollectedBy string
}
+type Stats2023 struct {
+ TeamNumber string `gorm:"primaryKey"`
+ MatchNumber int32 `gorm:"primaryKey"`
+ SetNumber int32 `gorm:"primaryKey"`
+ CompLevel string `gorm:"primaryKey"`
+ StartingQuadrant int32
+ LowCubesAuto, MiddleCubesAuto, HighCubesAuto, CubesDroppedAuto int32
+ LowConesAuto, MiddleConesAuto, HighConesAuto, ConesDroppedAuto int32
+ LowCubes, MiddleCubes, HighCubes, CubesDropped int32
+ LowCones, MiddleCones, HighCones, ConesDropped int32
+ AvgCycle int32
+ // The username of the person who collected these statistics.
+ // "unknown" if submitted without logging in.
+ // Empty if the stats have not yet been collected.
+ CollectedBy string
+}
+
+type Action struct {
+ TeamNumber string `gorm:"primaryKey"`
+ MatchNumber int32 `gorm:"primaryKey"`
+ SetNumber int32 `gorm:"primaryKey"`
+ CompLevel string `gorm:"primaryKey"`
+ CompletedAction []byte
+ // This contains a serialized scouting.webserver.requests.ActionType flatbuffer.
+ TimeStamp int32 `gorm:"primaryKey"`
+ CollectedBy string
+}
+
type NotesData struct {
ID uint `gorm:"primaryKey"`
TeamNumber int32
@@ -113,7 +141,7 @@
return nil, errors.New(fmt.Sprint("Failed to connect to postgres: ", err))
}
- err = database.AutoMigrate(&Match{}, &Shift{}, &Stats{}, &NotesData{}, &Ranking{}, &DriverRankingData{})
+ err = database.AutoMigrate(&Match{}, &Shift{}, &Stats{}, &Stats2023{}, &Action{}, &NotesData{}, &Ranking{}, &DriverRankingData{})
if err != nil {
database.Delete()
return nil, errors.New(fmt.Sprint("Failed to create/migrate tables: ", err))
@@ -148,6 +176,13 @@
return result.Error
}
+func (database *Database) AddAction(a Action) error {
+ result := database.Clauses(clause.OnConflict{
+ UpdateAll: true,
+ }).Create(&a)
+ return result.Error
+}
+
func (database *Database) AddToStats(s Stats) error {
matches, err := database.queryMatches(s.TeamNumber)
if err != nil {
@@ -176,6 +211,28 @@
return result.Error
}
+func (database *Database) AddToStats2023(s Stats2023) error {
+ matches, err := database.QueryMatchesString(s.TeamNumber)
+ if err != nil {
+ return err
+ }
+ foundMatch := false
+ for _, match := range matches {
+ if match.MatchNumber == s.MatchNumber {
+ foundMatch = true
+ break
+ }
+ }
+ if !foundMatch {
+ return errors.New(fmt.Sprint(
+ "Failed to find team ", s.TeamNumber,
+ " in match ", s.MatchNumber, " in the schedule."))
+ }
+
+ result := database.Create(&s)
+ return result.Error
+}
+
func (database *Database) AddOrUpdateRankings(r Ranking) error {
result := database.Clauses(clause.OnConflict{
UpdateAll: true,
@@ -207,6 +264,12 @@
return shifts, result.Error
}
+func (database *Database) ReturnActions() ([]Action, error) {
+ var actions []Action
+ result := database.Find(&actions)
+ return actions, result.Error
+}
+
// Packs the stats. This really just consists of taking the individual auto
// ball booleans and turning them into an array. The individual booleans are
// cleared so that they don't affect struct comparisons.
@@ -249,6 +312,14 @@
return matches, result.Error
}
+func (database *Database) QueryMatchesString(teamNumber_ string) ([]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_).
+ Find(&matches)
+ return matches, result.Error
+}
+
func (database *Database) QueryAllShifts(matchNumber_ int) ([]Shift, error) {
var shifts []Shift
result := database.Where("match_number = ?", matchNumber_).Find(&shifts)
@@ -265,6 +336,13 @@
return stats, result.Error
}
+func (database *Database) QueryActions(teamNumber_ int) ([]Action, error) {
+ var actions []Action
+ result := database.
+ Where("team_number = ?", teamNumber_).Find(&actions)
+ return actions, result.Error
+}
+
func (database *Database) QueryNotes(TeamNumber int32) ([]string, error) {
var rawNotes []NotesData
result := database.Where("team_number = ?", TeamNumber).Find(&rawNotes)
diff --git a/scouting/db/db_test.go b/scouting/db/db_test.go
index 47a0a90..9e85651 100644
--- a/scouting/db/db_test.go
+++ b/scouting/db/db_test.go
@@ -623,6 +623,54 @@
}
}
+func TestReturnActionsDB(t *testing.T) {
+ fixture := createDatabase(t)
+ defer fixture.TearDown()
+ correct := []Action{
+ Action{
+ TeamNumber: "1235", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0000, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1236", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0321, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1237", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0222, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1238", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0110, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1239", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0004, CollectedBy: "",
+ },
+ Action{
+ TeamNumber: "1233", MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ CompletedAction: []byte(""), TimeStamp: 0004, CollectedBy: "",
+ },
+ }
+
+ err := fixture.db.AddToMatch(Match{
+ MatchNumber: 94, SetNumber: 1, CompLevel: "quals",
+ R1: 1235, R2: 1236, R3: 1237, B1: 1238, B2: 1239, B3: 1233})
+ check(t, err, "Failed to add match")
+
+ for i := 0; i < len(correct); i++ {
+ err = fixture.db.AddAction(correct[i])
+ check(t, err, fmt.Sprint("Failed to add to actions ", i))
+ }
+
+ got, err := fixture.db.ReturnActions()
+ check(t, err, "Failed ReturnActions()")
+
+ if !reflect.DeepEqual(correct, got) {
+ t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
+ }
+}
+
func TestRankingsDbUpdate(t *testing.T) {
fixture := createDatabase(t)
defer fixture.TearDown()
diff --git a/scouting/webserver/requests/BUILD b/scouting/webserver/requests/BUILD
index 8cc59a9..9eb207d 100644
--- a/scouting/webserver/requests/BUILD
+++ b/scouting/webserver/requests/BUILD
@@ -24,6 +24,8 @@
"//scouting/webserver/requests/messages:request_notes_for_team_response_go_fbs",
"//scouting/webserver/requests/messages:request_shift_schedule_go_fbs",
"//scouting/webserver/requests/messages:request_shift_schedule_response_go_fbs",
+ "//scouting/webserver/requests/messages:submit_actions_go_fbs",
+ "//scouting/webserver/requests/messages:submit_actions_response_go_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_go_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_response_go_fbs",
"//scouting/webserver/requests/messages:submit_driver_ranking_go_fbs",
diff --git a/scouting/webserver/requests/messages/BUILD b/scouting/webserver/requests/messages/BUILD
index 57b7ae3..2e118d5 100644
--- a/scouting/webserver/requests/messages/BUILD
+++ b/scouting/webserver/requests/messages/BUILD
@@ -25,6 +25,8 @@
"submit_shift_schedule_response",
"submit_driver_ranking",
"submit_driver_ranking_response",
+ "submit_actions",
+ "submit_actions_response",
)
filegroup(
diff --git a/scouting/webserver/requests/messages/request_data_scouting_response.fbs b/scouting/webserver/requests/messages/request_data_scouting_response.fbs
index 071a848..6048210 100644
--- a/scouting/webserver/requests/messages/request_data_scouting_response.fbs
+++ b/scouting/webserver/requests/messages/request_data_scouting_response.fbs
@@ -49,4 +49,4 @@
stats_list:[Stats] (id:0);
}
-root_type RequestDataScoutingResponse;
+root_type RequestDataScoutingResponse;
\ No newline at end of file
diff --git a/scouting/webserver/requests/messages/submit_actions.fbs b/scouting/webserver/requests/messages/submit_actions.fbs
new file mode 100644
index 0000000..9d9efa4
--- /dev/null
+++ b/scouting/webserver/requests/messages/submit_actions.fbs
@@ -0,0 +1,53 @@
+namespace scouting.webserver.requests;
+
+table StartMatchAction {
+ position:int (id:0);
+}
+
+enum ObjectType: short {
+ kCube,
+ kCone
+}
+
+enum ScoreLevel: short {
+ kLow,
+ kMiddle,
+ kHigh
+}
+
+table PickupObjectAction {
+ object_type:ObjectType (id:0);
+ auto:bool (id:1);
+}
+
+table PlaceObjectAction {
+ object_type:ObjectType (id:0);
+ score_level:ScoreLevel (id:1);
+ auto:bool (id:2);
+}
+
+table RobotDeathAction {
+ robot_on:bool (id:0);
+}
+
+table EndMatchAction {
+}
+
+union ActionType {
+ StartMatchAction,
+ PickupObjectAction,
+ PlaceObjectAction,
+ RobotDeathAction,
+ EndMatchAction
+}
+
+table Action {
+ timestamp:int (id:0);
+ action_taken:ActionType (id:2);
+}
+
+table SubmitActions {
+ actions_list:[Action] (id:0);
+}
+
+root_type SubmitActions;
\ No newline at end of file
diff --git a/scouting/webserver/requests/messages/submit_actions_response.fbs b/scouting/webserver/requests/messages/submit_actions_response.fbs
new file mode 100644
index 0000000..4079914
--- /dev/null
+++ b/scouting/webserver/requests/messages/submit_actions_response.fbs
@@ -0,0 +1,7 @@
+namespace scouting.webserver.requests;
+
+table SubmitActionsResponse {
+ // empty response
+}
+
+root_type SubmitActionsResponse;
\ No newline at end of file
diff --git a/scouting/webserver/requests/requests.go b/scouting/webserver/requests/requests.go
index 76a26de..99b5459 100644
--- a/scouting/webserver/requests/requests.go
+++ b/scouting/webserver/requests/requests.go
@@ -27,6 +27,8 @@
"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"
"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_actions"
+ "github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_actions_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_data_scouting_response"
"github.com/frc971/971-Robot-Code/scouting/webserver/requests/messages/submit_driver_ranking"
@@ -61,6 +63,8 @@
type SubmitShiftScheduleResponseT = submit_shift_schedule_response.SubmitShiftScheduleResponseT
type SubmitDriverRanking = submit_driver_ranking.SubmitDriverRanking
type SubmitDriverRankingResponseT = submit_driver_ranking_response.SubmitDriverRankingResponseT
+type SubmitActions = submit_actions.SubmitActions
+type SubmitActionsResponseT = submit_actions_response.SubmitActionsResponseT
// The interface we expect the database abstraction to conform to.
// We use an interface here because it makes unit testing easier.
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/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
index 4976efa..0b2c5f3 100644
--- a/y2020/control_loops/superstructure/turret/aiming.cc
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -9,9 +9,9 @@
namespace turret {
using frc971::control_loops::Pose;
-using frc971::control_loops::aiming::TurretGoal;
-using frc971::control_loops::aiming::ShotConfig;
using frc971::control_loops::aiming::RobotState;
+using frc971::control_loops::aiming::ShotConfig;
+using frc971::control_loops::aiming::TurretGoal;
// Shooting-on-the-fly concept:
// The current way that we manage shooting-on-the fly endeavors to be reasonably
@@ -129,7 +129,9 @@
return target;
}
-Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
+Aimer::Aimer()
+ : goal_(MakePrefilledGoal()),
+ Tlr_to_la_(drivetrain::GetDrivetrainConfig().Tlr_to_la()) {}
void Aimer::Update(const Status *status, aos::Alliance alliance,
WrapMode wrap_mode, ShotMode shot_mode) {
@@ -145,9 +147,8 @@
// robot. All of this would be helped by just doing this work in the Localizer
// itself.
const Eigen::Vector2d linear_angular =
- drivetrain::GetDrivetrainConfig().Tlr_to_la() *
- Eigen::Vector2d(status->localizer()->left_velocity(),
- status->localizer()->right_velocity());
+ Tlr_to_la_ * Eigen::Vector2d(status->localizer()->left_velocity(),
+ status->localizer()->right_velocity());
const double xdot = linear_angular(0) * std::cos(status->theta());
const double ydot = linear_angular(0) * std::sin(status->theta());
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
index 217085c..f38113d 100644
--- a/y2020/control_loops/superstructure/turret/aiming.h
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -64,6 +64,8 @@
// Real-world distance to the target.
double target_distance_ = 0.0; // meters
double inner_port_angle_ = 0.0; // radians
+
+ Eigen::Matrix<double, 2, 2> Tlr_to_la_;
};
} // namespace turret
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
index a01f47d..fe71e31 100644
--- a/y2020/control_loops/superstructure/turret/aiming_test.cc
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -12,12 +12,24 @@
namespace turret {
namespace testing {
+class TeamNumberEnvironment : public ::testing::Environment {
+ public:
+ ~TeamNumberEnvironment() override {}
+
+ // Override this to define how to set up the environment.
+ void SetUp() override { aos::network::OverrideTeamNumber(971); }
+
+ // Override this to define how to tear down the environment.
+ void TearDown() override {}
+};
+
+::testing::Environment *const team_number_env =
+ ::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
+
using frc971::control_loops::Pose;
class AimerTest : public ::testing::Test {
public:
- AimerTest() { aos::network::OverrideTeamNumber(971); }
-
typedef Aimer::Goal Goal;
typedef Aimer::Status Status;
struct StatusData {
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/vision/calibrate_extrinsics.cc b/y2022/vision/calibrate_extrinsics.cc
index b5005fe..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"
@@ -21,6 +22,8 @@
DEFINE_string(target_type, "charuco",
"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,6 +80,11 @@
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 == "aruco") {
target_type = TargetType::kAruco;
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/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index d20a247..cbfd3f6 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -9,15 +9,19 @@
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);
- }),
+ 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();
@@ -151,8 +155,8 @@
aos::monotonic_clock::now();
VLOG(1) << "Took "
- << std::chrono::duration<double>(after_pose_estimation -
- before_pose_estimation)
+ << chrono::duration<double>(after_pose_estimation -
+ before_pose_estimation)
.count()
<< " seconds for pose estimation";
}
@@ -164,8 +168,7 @@
timeprofile_display(tag_detector_->tp);
- VLOG(1) << "Took "
- << std::chrono::duration<double>(end_time - start_time).count()
+ VLOG(1) << "Took " << chrono::duration<double>(end_time - start_time).count()
<< " seconds to detect overall";
return results;
diff --git a/y2023/vision/viewer.cc b/y2023/vision/viewer.cc
index 08def5b..68495b1 100644
--- a/y2023/vision/viewer.cc
+++ b/y2023/vision/viewer.cc
@@ -14,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 {
@@ -78,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_pi_template.json b/y2023/y2023_pi_template.json
index 592aa14..cf87798 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -164,7 +164,7 @@
"type": "frc971.vision.CameraImage",
"source_node": "pi{{ NUM }}",
"frequency": 40,
- "max_size": 4200000,
+ "max_size": 1843456,
"num_readers": 4,
"read_method": "PIN",
"num_senders": 18
@@ -360,6 +360,15 @@
"nodes": [
"pi{{ NUM }}"
]
+ },
+ {
+ "name": "aprilrobotics",
+ "executable_name": "aprilrobotics",
+ "args": ["--enable_ftrace"],
+ "user": "pi",
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
}
],
"maps": [