Merge "Refactor & test foxglove image converter"
diff --git a/aos/BUILD b/aos/BUILD
index 9768753..b8468bd 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -157,7 +157,10 @@
],
target_compatible_with = ["@platforms//os:linux"],
deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
"//aos/time",
+ "//aos/util:top",
"@com_github_google_glog//:glog",
],
)
diff --git a/aos/dump_rtprio.cc b/aos/dump_rtprio.cc
index 8020857..d36a1c0 100644
--- a/aos/dump_rtprio.cc
+++ b/aos/dump_rtprio.cc
@@ -18,9 +18,14 @@
#include <set>
#include <string>
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/util/top.h"
#include "aos/time/time.h"
#include "glog/logging.h"
+DEFINE_string(config, "aos_config.json", "File path of aos configuration");
+
namespace {
const char *policy_string(uint32_t policy) {
@@ -52,17 +57,7 @@
return str.substr(0, str.size() - 1);
}
-int find_pid_max() {
- int r;
- FILE *pid_max_file = fopen("/proc/sys/kernel/pid_max", "r");
- PCHECK(pid_max_file != nullptr)
- << ": Failed to open /proc/sys/kernel/pid_max";
- CHECK_EQ(1, fscanf(pid_max_file, "%d", &r));
- PCHECK(fclose(pid_max_file) == 0);
- return r;
-}
-
-cpu_set_t find_all_cpus() {
+cpu_set_t FindAllCpus() {
long nproc = sysconf(_SC_NPROCESSORS_CONF);
PCHECK(nproc != -1);
cpu_set_t r;
@@ -253,47 +248,63 @@
} // namespace
-int main() {
- const int pid_max = find_pid_max();
- const cpu_set_t all_cpus = find_all_cpus();
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
- std::multiset<Thread> threads;
+ aos::ShmEventLoop event_loop(&config.message());
+ event_loop.SkipTimingReport();
+ event_loop.SkipAosLog();
+ aos::util::Top top(&event_loop);
+ top.set_track_top_processes(true);
- for (int i = 0; i < pid_max; ++i) {
- bool not_there = false;
+ const cpu_set_t all_cpus = FindAllCpus();
- const cpu_set_t cpu_mask = find_cpu_mask(i, ¬_there);
- const sched_param param = find_sched_param(i, ¬_there);
- const int scheduler = find_scheduler(i, ¬_there);
- const ::std::string exe = find_exe(i, ¬_there);
- const int nice_value = find_nice_value(i, ¬_there);
+ top.set_on_reading_update([&]() {
+ std::multiset<Thread> threads;
- int ppid = 0, sid = 0;
- read_stat(i, &ppid, &sid, ¬_there);
+ for (const std::pair<const pid_t, aos::util::Top::ProcessReadings>
+ &reading : top.readings()) {
+ pid_t tid = reading.first;
+ bool not_there = false;
- int pgrp = 0;
- ::std::string name;
- read_status(i, ppid, &pgrp, &name, ¬_there);
+ const cpu_set_t cpu_mask = find_cpu_mask(tid, ¬_there);
+ const sched_param param = find_sched_param(tid, ¬_there);
+ const int scheduler = find_scheduler(tid, ¬_there);
+ const ::std::string exe = find_exe(tid, ¬_there);
+ const int nice_value = find_nice_value(tid, ¬_there);
- if (not_there) continue;
+ int ppid = 0, sid = 0;
+ read_stat(tid, &ppid, &sid, ¬_there);
- const char *cpu_mask_string =
- CPU_EQUAL(&cpu_mask, &all_cpus) ? "all" : "???";
+ int pgrp = 0;
+ ::std::string name;
+ read_status(tid, ppid, &pgrp, &name, ¬_there);
- threads.emplace(Thread{.policy = static_cast<uint32_t>(scheduler),
- .exe = exe,
- .name = name,
- .cpu_mask = cpu_mask_string,
- .nice_value = nice_value,
- .sched_priority = param.sched_priority,
- .tid = i,
- .pid = pgrp,
- .ppid = ppid,
- .sid = sid});
- }
+ if (not_there) continue;
- printf("exe,name,cpumask,policy,nice,priority,tid,pid,ppid,sid\n");
- for (const auto &t : threads) {
- t.Print();
- }
+ const char *cpu_mask_string =
+ CPU_EQUAL(&cpu_mask, &all_cpus) ? "all" : "???";
+
+ threads.emplace(Thread{.policy = static_cast<uint32_t>(scheduler),
+ .exe = exe,
+ .name = name,
+ .cpu_mask = cpu_mask_string,
+ .nice_value = nice_value,
+ .sched_priority = param.sched_priority,
+ .tid = tid,
+ .pid = pgrp,
+ .ppid = ppid,
+ .sid = sid});
+ }
+
+ printf("exe,name,cpumask,policy,nice,priority,tid,pid,ppid,sid\n");
+ for (const auto &t : threads) {
+ t.Print();
+ }
+ event_loop.Exit();
+ });
+
+ event_loop.Run();
}
diff --git a/aos/events/event_loop.cc b/aos/events/event_loop.cc
index 6b5b601..f5e9f51 100644
--- a/aos/events/event_loop.cc
+++ b/aos/events/event_loop.cc
@@ -115,11 +115,7 @@
const monotonic_clock::time_point monotonic_now =
event_loop_->monotonic_now();
phased_loop_.Reset(monotonic_now);
- Reschedule(
- [this](monotonic_clock::time_point sleep_time) {
- Schedule(sleep_time);
- },
- monotonic_now);
+ Reschedule(monotonic_now);
// Reschedule here will count cycles elapsed before now, and then the
// reschedule before running the handler will count the time that elapsed
// then. So clear the count here.
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 471b625..23250e1 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -511,26 +511,61 @@
Ftrace ftrace_;
};
-// Interface for phased loops. They are built on timers.
+// Interface for phased loops. They are built on timers.
class PhasedLoopHandler {
public:
virtual ~PhasedLoopHandler();
- // Sets the interval and offset. Any changes to interval and offset only take
- // effect when the handler finishes running.
- void set_interval_and_offset(const monotonic_clock::duration interval,
- const monotonic_clock::duration offset) {
- phased_loop_.set_interval_and_offset(interval, offset);
+ // Sets the interval and offset. Any changes to interval and offset only take
+ // effect when the handler finishes running or upon a call to Reschedule().
+ //
+ // The precise semantics of the monotonic_now parameter are defined in
+ // phased_loop.h. The way that it gets used in this interface is to control
+ // what the cycles elapsed counter will read on the following call to your
+ // handler. In an idealized simulation environment, if you call
+ // set_interval_and_offset() during the phased loop offset without setting
+ // monotonic_now, then you should always see a count of 1 on the next cycle.
+ //
+ // With the default behavior (this is called inside your handler and with
+ // monotonic_now = nullopt), the next phased loop call will occur at most
+ // interval time after the current time. Note that in many cases this will
+ // *not* be the preferred behavior (in most cases, you would likely aim to
+ // keep the average frequency of the calls reasonably consistent).
+ //
+ // A combination of the monotonic_now parameter and manually calling
+ // Reschedule() outside of the phased loop handler can be used to alter the
+ // behavior of cycles_elapsed. For the default behavior, you can set
+ // monotonic_now to the current time. If you call set_interval_and_offset()
+ // and Reschedule() with the same monotonic_now, that will cause the next
+ // callback to occur in the range (monotonic_now, monotonic_now + interval]
+ // and get called with a count of 1 (if the event is actually able to happen
+ // when it is scheduled to). E.g., if you are just adjusting the phased loop
+ // offset (but not the interval) and want to maintain a consistent frequency,
+ // you may call these functions with monotonic_now = now + interval / 2.
+ void set_interval_and_offset(
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset,
+ std::optional<monotonic_clock::time_point> monotonic_now = std::nullopt) {
+ phased_loop_.set_interval_and_offset(interval, offset, monotonic_now);
}
- // Sets and gets the name of the timer. Set this if you want a descriptive
+ // Reruns the scheduler for the phased loop, scheduling the next callback at
+ // the next available time after monotonic_now. This allows
+ // set_interval_and_offset() to be called and have the changes take effect
+ // before the next handler finishes. This will have no effect if run during
+ // the phased loop's own handler.
+ void Reschedule(monotonic_clock::time_point monotonic_now) {
+ cycles_elapsed_ += phased_loop_.Iterate(monotonic_now);
+ Schedule(phased_loop_.sleep_time());
+ }
+
+ // Sets and gets the name of the timer. Set this if you want a descriptive
// name in the timing report.
void set_name(std::string_view name) { name_ = std::string(name); }
const std::string_view name() const { return name_; }
protected:
- void Call(std::function<monotonic_clock::time_point()> get_time,
- std::function<void(monotonic_clock::time_point)> schedule);
+ void Call(std::function<monotonic_clock::time_point()> get_time);
PhasedLoopHandler(EventLoop *event_loop, std::function<void(int)> fn,
const monotonic_clock::duration interval,
@@ -539,12 +574,6 @@
private:
friend class EventLoop;
- void Reschedule(std::function<void(monotonic_clock::time_point)> schedule,
- monotonic_clock::time_point monotonic_now) {
- cycles_elapsed_ += phased_loop_.Iterate(monotonic_now);
- schedule(phased_loop_.sleep_time());
- }
-
virtual void Schedule(monotonic_clock::time_point sleep_time) = 0;
EventLoop *event_loop_;
diff --git a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc
index 0c8e41e..07bd6d4 100644
--- a/aos/events/event_loop_param_test.cc
+++ b/aos/events/event_loop_param_test.cc
@@ -1922,6 +1922,298 @@
EXPECT_GT(expected_times[expected_times.size() / 2], average_time - kEpsilon);
}
+// Tests that a phased loop responds correctly to a changing offset; sweep
+// across a variety of potential offset changes, to ensure that we are
+// exercising a variety of potential cases.
+TEST_P(AbstractEventLoopTest, PhasedLoopChangingOffsetSweep) {
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+ const int kCount = 5;
+
+ auto loop1 = MakePrimary();
+
+ std::vector<aos::monotonic_clock::duration> offset_options;
+ for (int ii = 0; ii < kCount; ++ii) {
+ offset_options.push_back(ii * kInterval / kCount);
+ }
+ std::vector<aos::monotonic_clock::duration> offset_sweep;
+ // Run over all the pair-wise combinations of offsets.
+ for (int ii = 0; ii < kCount; ++ii) {
+ for (int jj = 0; jj < kCount; ++jj) {
+ offset_sweep.push_back(offset_options.at(ii));
+ offset_sweep.push_back(offset_options.at(jj));
+ }
+ }
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ // Run kCount iterations.
+ size_t counter = 0;
+ phased_loop = loop1->AddPhasedLoop(
+ [&phased_loop, &expected_times, &loop1, this, kInterval, &counter,
+ offset_sweep](int count) {
+ EXPECT_EQ(count, 1);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ counter++;
+
+ if (counter == offset_sweep.size()) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+
+ phased_loop->set_interval_and_offset(kInterval,
+ offset_sweep.at(counter));
+ },
+ kInterval, offset_sweep.at(0));
+
+ Run();
+ ASSERT_EQ(expected_times.size(), offset_sweep.size());
+ for (size_t ii = 1; ii < expected_times.size(); ++ii) {
+ EXPECT_LE(expected_times.at(ii) - expected_times.at(ii - 1), kInterval);
+ }
+}
+
+// Tests that a phased loop responds correctly to being rescheduled with now
+// equal to a time in the past.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleInPast) {
+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+ auto loop1 = MakePrimary();
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ int expected_count = 1;
+
+ // Set up a timer that will get run immediately after the phased loop and
+ // which will attempt to reschedule the phased loop to just before now. This
+ // should succeed, but will result in 0 cycles elapsing.
+ TimerHandler *manager_timer =
+ loop1->AddTimer([&phased_loop, &loop1, &expected_count, this]() {
+ if (expected_count == 0) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+ phased_loop->Reschedule(loop1->context().monotonic_event_time -
+ std::chrono::nanoseconds(1));
+ expected_count = 0;
+ });
+
+ phased_loop = loop1->AddPhasedLoop(
+ [&expected_count, &expected_times, &loop1, manager_timer](int count) {
+ EXPECT_EQ(count, expected_count);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ manager_timer->Setup(loop1->context().monotonic_event_time);
+ },
+ kInterval, kOffset);
+ phased_loop->set_name("Test loop");
+ manager_timer->set_name("Manager timer");
+
+ Run();
+
+ ASSERT_EQ(2u, expected_times.size());
+ ASSERT_EQ(expected_times[0], expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to being rescheduled at the time
+// when it should be triggering (it should kick the trigger to the next cycle).
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleNow) {
+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+ auto loop1 = MakePrimary();
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ bool should_exit = false;
+ // Set up a timer that will get run immediately after the phased loop and
+ // which will attempt to reschedule the phased loop to now. This should
+ // succeed, but will result in no change to the expected behavior (since this
+ // is the same thing that is actually done internally).
+ TimerHandler *manager_timer =
+ loop1->AddTimer([&phased_loop, &loop1, &should_exit, this]() {
+ if (should_exit) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+ phased_loop->Reschedule(loop1->context().monotonic_event_time);
+ should_exit = true;
+ });
+
+ phased_loop = loop1->AddPhasedLoop(
+ [&expected_times, &loop1, manager_timer](int count) {
+ EXPECT_EQ(count, 1);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ manager_timer->Setup(loop1->context().monotonic_event_time);
+ },
+ kInterval, kOffset);
+ phased_loop->set_name("Test loop");
+ manager_timer->set_name("Manager timer");
+
+ Run();
+
+ ASSERT_EQ(2u, expected_times.size());
+ ASSERT_EQ(expected_times[0] + kInterval, expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to being rescheduled at a time in
+// the distant future.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleFuture) {
+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+ auto loop1 = MakePrimary();
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ bool should_exit = false;
+ int expected_count = 1;
+ TimerHandler *manager_timer = loop1->AddTimer(
+ [&expected_count, &phased_loop, &loop1, &should_exit, this, kInterval]() {
+ if (should_exit) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+ expected_count = 10;
+ // Knock off 1 ns, since the scheduler rounds up when it is
+ // scheduled to exactly a loop time.
+ phased_loop->Reschedule(loop1->context().monotonic_event_time +
+ kInterval * expected_count -
+ std::chrono::nanoseconds(1));
+ should_exit = true;
+ });
+
+ phased_loop = loop1->AddPhasedLoop(
+ [&expected_times, &loop1, manager_timer, &expected_count](int count) {
+ EXPECT_EQ(count, expected_count);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ manager_timer->Setup(loop1->context().monotonic_event_time);
+ },
+ kInterval, kOffset);
+ phased_loop->set_name("Test loop");
+ manager_timer->set_name("Manager timer");
+
+ Run();
+
+ ASSERT_EQ(2u, expected_times.size());
+ ASSERT_EQ(expected_times[0] + expected_count * kInterval, expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to having its phase offset
+// incremented and then being scheduled after a set time, exercising a pattern
+// where a phased loop's offset is changed while trying to maintain the trigger
+// at a consistent period.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithLaterOffset) {
+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+ auto loop1 = MakePrimary();
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ bool should_exit = false;
+ TimerHandler *manager_timer = loop1->AddTimer(
+ [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
+ if (should_exit) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+ // Schedule the next callback to be strictly later than the current time
+ // + interval / 2, to ensure a consistent frequency.
+ monotonic_clock::time_point half_time =
+ loop1->context().monotonic_event_time + kInterval / 2;
+ phased_loop->set_interval_and_offset(
+ kInterval, kOffset + std::chrono::nanoseconds(1), half_time);
+ phased_loop->Reschedule(half_time);
+ should_exit = true;
+ });
+
+ phased_loop = loop1->AddPhasedLoop(
+ [&expected_times, &loop1, manager_timer](int count) {
+ EXPECT_EQ(1, count);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ manager_timer->Setup(loop1->context().monotonic_event_time);
+ },
+ kInterval, kOffset);
+ phased_loop->set_name("Test loop");
+ manager_timer->set_name("Manager timer");
+
+ Run();
+
+ ASSERT_EQ(2u, expected_times.size());
+ ASSERT_EQ(expected_times[0] + kInterval + std::chrono::nanoseconds(1),
+ expected_times[1]);
+}
+
+// Tests that a phased loop responds correctly to having its phase offset
+// decremented and then being scheduled after a set time, exercising a pattern
+// where a phased loop's offset is changed while trying to maintain the trigger
+// at a consistent period.
+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithEarlierOffset) {
+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
+
+ auto loop1 = MakePrimary();
+
+ std::vector<::aos::monotonic_clock::time_point> expected_times;
+
+ PhasedLoopHandler *phased_loop;
+
+ bool should_exit = false;
+ TimerHandler *manager_timer = loop1->AddTimer(
+ [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
+ if (should_exit) {
+ LOG(INFO) << "Exiting";
+ this->Exit();
+ return;
+ }
+ // Schedule the next callback to be strictly later than the current time
+ // + interval / 2, to ensure a consistent frequency.
+ const aos::monotonic_clock::time_point half_time =
+ loop1->context().monotonic_event_time + kInterval / 2;
+ phased_loop->set_interval_and_offset(
+ kInterval, kOffset - std::chrono::nanoseconds(1), half_time);
+ phased_loop->Reschedule(half_time);
+ should_exit = true;
+ });
+
+ phased_loop = loop1->AddPhasedLoop(
+ [&expected_times, &loop1, manager_timer](int count) {
+ EXPECT_EQ(1, count);
+ expected_times.push_back(loop1->context().monotonic_event_time);
+
+ manager_timer->Setup(loop1->context().monotonic_event_time);
+ },
+ kInterval, kOffset);
+ phased_loop->set_name("Test loop");
+ manager_timer->set_name("Manager timer");
+
+ Run();
+
+ ASSERT_EQ(2u, expected_times.size());
+ ASSERT_EQ(expected_times[0] + kInterval - std::chrono::nanoseconds(1),
+ expected_times[1]);
+}
+
// Tests that senders count correctly in the timing report.
TEST_P(AbstractEventLoopTest, SenderTimingReport) {
FLAGS_timing_report_ms = 1000;
diff --git a/aos/events/event_loop_param_test.cc.rej b/aos/events/event_loop_param_test.cc.rej
new file mode 100644
index 0000000..c03b83d
--- /dev/null
+++ b/aos/events/event_loop_param_test.cc.rej
@@ -0,0 +1,300 @@
+diff a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc (rejected hunks)
+@@ -1923,6 +1923,298 @@
+ EXPECT_GT(expected_times[expected_times.size() / 2], average_time - kEpsilon);
+ }
+
++// Tests that a phased loop responds correctly to a changing offset; sweep
++// across a variety of potential offset changes, to ensure that we are
++// exercising a variety of potential cases.
++TEST_P(AbstractEventLoopTest, PhasedLoopChangingOffsetSweep) {
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++ const int kCount = 5;
++
++ auto loop1 = MakePrimary();
++
++ std::vector<aos::monotonic_clock::duration> offset_options;
++ for (int ii = 0; ii < kCount; ++ii) {
++ offset_options.push_back(ii * kInterval / kCount);
++ }
++ std::vector<aos::monotonic_clock::duration> offset_sweep;
++ // Run over all the pair-wise combinations of offsets.
++ for (int ii = 0; ii < kCount; ++ii) {
++ for (int jj = 0; jj < kCount; ++jj) {
++ offset_sweep.push_back(offset_options.at(ii));
++ offset_sweep.push_back(offset_options.at(jj));
++ }
++ }
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ // Run kCount iterations.
++ int counter = 0;
++ phased_loop = loop1->AddPhasedLoop(
++ [&phased_loop, &expected_times, &loop1, this, kInterval, &counter,
++ offset_sweep](int count) {
++ EXPECT_EQ(count, 1);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ counter++;
++
++ if (counter == offset_sweep.size()) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++
++ phased_loop->set_interval_and_offset(kInterval,
++ offset_sweep.at(counter));
++ },
++ kInterval, offset_sweep.at(0));
++
++ Run();
++ ASSERT_EQ(expected_times.size(), offset_sweep.size());
++ for (size_t ii = 1; ii < expected_times.size(); ++ii) {
++ EXPECT_LE(expected_times.at(ii) - expected_times.at(ii - 1), kInterval);
++ }
++}
++
++// Tests that a phased loop responds correctly to being rescheduled with now
++// equal to a time in the past.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleInPast) {
++ const chrono::milliseconds kOffset = chrono::milliseconds(400);
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++ auto loop1 = MakePrimary();
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ int expected_count = 1;
++
++ // Set up a timer that will get run immediately after the phased loop and
++ // which will attempt to reschedule the phased loop to just before now. This
++ // should succeed, but will result in 0 cycles elapsing.
++ TimerHandler *manager_timer =
++ loop1->AddTimer([&phased_loop, &loop1, &expected_count, this]() {
++ if (expected_count == 0) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++ phased_loop->Reschedule(loop1->context().monotonic_event_time -
++ std::chrono::nanoseconds(1));
++ expected_count = 0;
++ });
++
++ phased_loop = loop1->AddPhasedLoop(
++ [&expected_count, &expected_times, &loop1, manager_timer](int count) {
++ EXPECT_EQ(count, expected_count);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ manager_timer->Setup(loop1->context().monotonic_event_time);
++ },
++ kInterval, kOffset);
++ phased_loop->set_name("Test loop");
++ manager_timer->set_name("Manager timer");
++
++ Run();
++
++ ASSERT_EQ(2u, expected_times.size());
++ ASSERT_EQ(expected_times[0], expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to being rescheduled at the time
++// when it should be triggering (it should kick the trigger to the next cycle).
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleNow) {
++ const chrono::milliseconds kOffset = chrono::milliseconds(400);
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++ auto loop1 = MakePrimary();
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ bool should_exit = false;
++ // Set up a timer that will get run immediately after the phased loop and
++ // which will attempt to reschedule the phased loop to now. This should
++ // succeed, but will result in no change to the expected behavior (since this
++ // is the same thing that is actually done internally).
++ TimerHandler *manager_timer =
++ loop1->AddTimer([&phased_loop, &loop1, &should_exit, this]() {
++ if (should_exit) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++ phased_loop->Reschedule(loop1->context().monotonic_event_time);
++ should_exit = true;
++ });
++
++ phased_loop = loop1->AddPhasedLoop(
++ [&expected_times, &loop1, manager_timer](int count) {
++ EXPECT_EQ(count, 1);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ manager_timer->Setup(loop1->context().monotonic_event_time);
++ },
++ kInterval, kOffset);
++ phased_loop->set_name("Test loop");
++ manager_timer->set_name("Manager timer");
++
++ Run();
++
++ ASSERT_EQ(2u, expected_times.size());
++ ASSERT_EQ(expected_times[0] + kInterval, expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to being rescheduled at a time in
++// the distant future.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleFuture) {
++ const chrono::milliseconds kOffset = chrono::milliseconds(400);
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++ auto loop1 = MakePrimary();
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ bool should_exit = false;
++ int expected_count = 1;
++ TimerHandler *manager_timer = loop1->AddTimer(
++ [&expected_count, &phased_loop, &loop1, &should_exit, this, kInterval]() {
++ if (should_exit) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++ expected_count = 10;
++ // Knock off 1 ns, since the scheduler rounds up when it is
++ // scheduled to exactly a loop time.
++ phased_loop->Reschedule(loop1->context().monotonic_event_time +
++ kInterval * expected_count -
++ std::chrono::nanoseconds(1));
++ should_exit = true;
++ });
++
++ phased_loop = loop1->AddPhasedLoop(
++ [&expected_times, &loop1, manager_timer, &expected_count](int count) {
++ EXPECT_EQ(count, expected_count);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ manager_timer->Setup(loop1->context().monotonic_event_time);
++ },
++ kInterval, kOffset);
++ phased_loop->set_name("Test loop");
++ manager_timer->set_name("Manager timer");
++
++ Run();
++
++ ASSERT_EQ(2u, expected_times.size());
++ ASSERT_EQ(expected_times[0] + expected_count * kInterval, expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to having its phase offset
++// incremented and then being scheduled after a set time, exercising a pattern
++// where a phased loop's offset is changed while trying to maintain the trigger
++// at a consistent period.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithLaterOffset) {
++ const chrono::milliseconds kOffset = chrono::milliseconds(400);
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++ auto loop1 = MakePrimary();
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ bool should_exit = false;
++ TimerHandler *manager_timer = loop1->AddTimer(
++ [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
++ if (should_exit) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++ // Schedule the next callback to be strictly later than the current time
++ // + interval / 2, to ensure a consistent frequency.
++ monotonic_clock::time_point half_time =
++ loop1->context().monotonic_event_time + kInterval / 2;
++ phased_loop->set_interval_and_offset(
++ kInterval, kOffset + std::chrono::nanoseconds(1), half_time);
++ phased_loop->Reschedule(half_time);
++ should_exit = true;
++ });
++
++ phased_loop = loop1->AddPhasedLoop(
++ [&expected_times, &loop1, manager_timer](int count) {
++ EXPECT_EQ(1, count);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ manager_timer->Setup(loop1->context().monotonic_event_time);
++ },
++ kInterval, kOffset);
++ phased_loop->set_name("Test loop");
++ manager_timer->set_name("Manager timer");
++
++ Run();
++
++ ASSERT_EQ(2u, expected_times.size());
++ ASSERT_EQ(expected_times[0] + kInterval + std::chrono::nanoseconds(1),
++ expected_times[1]);
++}
++
++// Tests that a phased loop responds correctly to having its phase offset
++// decremented and then being scheduled after a set time, exercising a pattern
++// where a phased loop's offset is changed while trying to maintain the trigger
++// at a consistent period.
++TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithEarlierOffset) {
++ const chrono::milliseconds kOffset = chrono::milliseconds(400);
++ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
++
++ auto loop1 = MakePrimary();
++
++ std::vector<::aos::monotonic_clock::time_point> expected_times;
++
++ PhasedLoopHandler *phased_loop;
++
++ bool should_exit = false;
++ TimerHandler *manager_timer = loop1->AddTimer(
++ [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
++ if (should_exit) {
++ LOG(INFO) << "Exiting";
++ this->Exit();
++ return;
++ }
++ // Schedule the next callback to be strictly later than the current time
++ // + interval / 2, to ensure a consistent frequency.
++ const aos::monotonic_clock::time_point half_time =
++ loop1->context().monotonic_event_time + kInterval / 2;
++ phased_loop->set_interval_and_offset(
++ kInterval, kOffset - std::chrono::nanoseconds(1), half_time);
++ phased_loop->Reschedule(half_time);
++ should_exit = true;
++ });
++
++ phased_loop = loop1->AddPhasedLoop(
++ [&expected_times, &loop1, manager_timer](int count) {
++ EXPECT_EQ(1, count);
++ expected_times.push_back(loop1->context().monotonic_event_time);
++
++ manager_timer->Setup(loop1->context().monotonic_event_time);
++ },
++ kInterval, kOffset);
++ phased_loop->set_name("Test loop");
++ manager_timer->set_name("Manager timer");
++
++ Run();
++
++ ASSERT_EQ(2u, expected_times.size());
++ ASSERT_EQ(expected_times[0] + kInterval - std::chrono::nanoseconds(1),
++ expected_times[1]);
++}
++
+ // Tests that senders count correctly in the timing report.
+ TEST_P(AbstractEventLoopTest, SenderTimingReport) {
+ gflags::FlagSaver flag_saver;
diff --git a/aos/events/event_loop_tmpl.h b/aos/events/event_loop_tmpl.h
index d206bc1..39ab43e 100644
--- a/aos/events/event_loop_tmpl.h
+++ b/aos/events/event_loop_tmpl.h
@@ -238,8 +238,7 @@
}
inline void PhasedLoopHandler::Call(
- std::function<monotonic_clock::time_point()> get_time,
- std::function<void(monotonic_clock::time_point)> schedule) {
+ std::function<monotonic_clock::time_point()> get_time) {
// Read time directly to save a vtable indirection...
const monotonic_clock::time_point monotonic_start_time = get_time();
@@ -270,7 +269,7 @@
cycles_elapsed_ = 0;
// Schedule the next wakeup.
- schedule(phased_loop_.sleep_time());
+ Schedule(phased_loop_.sleep_time());
const monotonic_clock::time_point monotonic_end_time = get_time();
ftrace_.FormatMessage(
@@ -287,7 +286,7 @@
// If the handler took too long so we blew by the previous deadline, we
// want to just try for the next deadline. Reschedule.
if (monotonic_end_time > phased_loop_.sleep_time()) {
- Reschedule(schedule, monotonic_end_time);
+ Reschedule(monotonic_end_time);
}
}
diff --git a/aos/events/shm_event_loop.cc b/aos/events/shm_event_loop.cc
index 56d2357..89d0a9c 100644
--- a/aos/events/shm_event_loop.cc
+++ b/aos/events/shm_event_loop.cc
@@ -679,9 +679,7 @@
timerfd_.Read();
event_.Invalidate();
- Call(monotonic_clock::now, [this](monotonic_clock::time_point sleep_time) {
- Schedule(sleep_time);
- });
+ Call(monotonic_clock::now);
}
~ShmPhasedLoopHandler() override {
diff --git a/aos/events/simulated_event_loop.cc b/aos/events/simulated_event_loop.cc
index 22b9603..c679b21 100644
--- a/aos/events/simulated_event_loop.cc
+++ b/aos/events/simulated_event_loop.cc
@@ -1293,10 +1293,7 @@
{
ScopedMarkRealtimeRestorer rt(
simulated_event_loop_->runtime_realtime_priority() > 0);
- Call([monotonic_now]() { return monotonic_now; },
- [this](monotonic_clock::time_point sleep_time) {
- Schedule(sleep_time);
- });
+ Call([monotonic_now]() { return monotonic_now; });
simulated_event_loop_->ClearContext();
}
}
@@ -1312,6 +1309,7 @@
// The allocations in here are due to infrastructure and don't count in the no
// mallocs in RT code.
ScopedNotRealtime nrt;
+ simulated_event_loop_->RemoveEvent(&event_);
if (token_ != scheduler_->InvalidToken()) {
scheduler_->Deschedule(token_);
token_ = scheduler_->InvalidToken();
diff --git a/aos/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/phased_loop.cc b/aos/util/phased_loop.cc
index 53f6f4d..1f61775 100644
--- a/aos/util/phased_loop.cc
+++ b/aos/util/phased_loop.cc
@@ -17,15 +17,26 @@
void PhasedLoop::set_interval_and_offset(
const monotonic_clock::duration interval,
- const monotonic_clock::duration offset) {
+ const monotonic_clock::duration offset,
+ std::optional<monotonic_clock::time_point> monotonic_now) {
// Update last_time_ to the new offset so that we have an even interval
- last_time_ += offset - offset_;
+ // In doing so, set things so that last_time_ will only ever decrease on calls
+ // to set_interval_and_offset.
+ last_time_ += offset - offset_ -
+ (offset > offset_ ? interval : monotonic_clock::duration(0));
interval_ = interval;
offset_ = offset;
CHECK(offset_ >= monotonic_clock::duration(0));
CHECK(interval_ > monotonic_clock::duration(0));
CHECK(offset_ < interval_);
+ // Reset effectively clears the skipped iteration count and ensures that the
+ // last time is in the interval (monotonic_now - interval, monotonic_now],
+ // which means that a call to Iterate(monotonic_now) will return 1 and set a
+ // wakeup time after monotonic_now.
+ if (monotonic_now.has_value()) {
+ Iterate(monotonic_now.value());
+ }
}
monotonic_clock::duration PhasedLoop::OffsetFromIntervalAndTime(
diff --git a/aos/util/phased_loop.h b/aos/util/phased_loop.h
index 307b400..64a3302 100644
--- a/aos/util/phased_loop.h
+++ b/aos/util/phased_loop.h
@@ -1,6 +1,8 @@
#ifndef AOS_UTIL_PHASED_LOOP_H_
#define AOS_UTIL_PHASED_LOOP_H_
+#include <optional>
+
#include "aos/time/time.h"
namespace aos {
@@ -21,8 +23,30 @@
const monotonic_clock::duration offset = monotonic_clock::duration(0));
// Updates the offset and interval.
- void set_interval_and_offset(const monotonic_clock::duration interval,
- const monotonic_clock::duration offset);
+ //
+ // After a call to set_interval_and_offset with monotonic_now = nullopt, the
+ // following will hold, for any allowed values of interval and offset:
+ // auto original_time = loop.sleep_time();
+ // loop.set_interval_and_offset(interval, offset);
+ // CHECK_LE(loop.sleep_time(), original_time);
+ // CHECK_EQ(0, loop.Iterate(original_time));
+ //
+ // Note that this will not be the behavior that all (or even necessarily most)
+ // users want, since it doesn't necessarily preserve a "keep the iteration
+ // time as consistent as possible" concept. However, it *is* better defined
+ // than the alternative, where if you decrease the offset by, e.g., 1ms on a
+ // 100ms interval, then the behavior will vary depending on whather you are
+ // going from 0ms->999ms offset or from 1ms->0ms offset.
+ //
+ // If monotonic_now is set, then the following will hold:
+ // auto original_time = loop.sleep_time();
+ // loop.set_interval_and_offset(interval, offset, monotonic_now);
+ // CHECK_LE(loop.sleep_time(), monotonic_now);
+ // CHECK_EQ(0, loop.Iterate(monotonic_now));
+ void set_interval_and_offset(
+ const monotonic_clock::duration interval,
+ const monotonic_clock::duration offset,
+ std::optional<monotonic_clock::time_point> monotonic_now = std::nullopt);
// Computes the offset given an interval and a time that we should trigger.
static monotonic_clock::duration OffsetFromIntervalAndTime(
diff --git a/aos/util/phased_loop_test.cc b/aos/util/phased_loop_test.cc
index 5e85ea0..de4483c 100644
--- a/aos/util/phased_loop_test.cc
+++ b/aos/util/phased_loop_test.cc
@@ -1,6 +1,7 @@
#include "aos/util/phased_loop.h"
#include "aos/time/time.h"
+#include "glog/logging.h"
#include "gtest/gtest.h"
namespace aos {
@@ -230,7 +231,23 @@
ASSERT_EQ(5, loop.Iterate(last_time));
for (int i = 1; i < kCount; i++) {
const auto offset = kOffset - milliseconds(i);
- loop.set_interval_and_offset(kInterval, offset);
+ // First, set the interval/offset without specifying a "now". If we then
+ // attempt to Iterate() to the same time as the last iteration, this should
+ // always result in zero cycles elapsed.
+ {
+ const monotonic_clock::time_point original_time = loop.sleep_time();
+ loop.set_interval_and_offset(kInterval, offset);
+ EXPECT_EQ(original_time - milliseconds(1), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(last_time));
+ }
+
+ // Now, explicitly update/clear things to last_time. This should have the
+ // same behavior as not specifying a monotonic_now.
+ {
+ loop.set_interval_and_offset(kInterval, offset, last_time);
+ EXPECT_EQ(0, loop.Iterate(last_time));
+ }
+
const auto next_time = last_time - milliseconds(1) + kAllIterationsInterval;
EXPECT_EQ(kIterations, loop.Iterate(next_time));
last_time = next_time;
@@ -238,6 +255,43 @@
}
// Tests that the phased loop is correctly adjusting when the offset is
+// incremented multiple times.
+TEST_F(PhasedLoopTest, IncrementingOffset) {
+ constexpr int kCount = 5;
+ constexpr int kIterations = 10;
+ const auto kOffset = milliseconds(0);
+ const auto kInterval = milliseconds(1000);
+ const auto kAllIterationsInterval = kInterval * kIterations;
+
+ PhasedLoop loop(kInterval, monotonic_clock::epoch(), kOffset);
+ auto last_time = monotonic_clock::epoch() + kOffset + (kInterval * 3);
+ ASSERT_EQ(4, loop.Iterate(last_time));
+ for (int i = 1; i < kCount; i++) {
+ const auto offset = kOffset + milliseconds(i);
+ {
+ const monotonic_clock::time_point original_time = loop.sleep_time();
+ loop.set_interval_and_offset(kInterval, offset);
+ EXPECT_EQ(original_time - kInterval + milliseconds(1), loop.sleep_time());
+ EXPECT_EQ(0, loop.Iterate(last_time));
+ }
+ // Now, explicitly update/clear things to a set time. We add a milliseconds
+ // so that when we call Iterate() next we actually get the expected number
+ // of iterations (otherwise, there is an iteration that would happen at
+ // last_time + 1 that gets counted, which is correct behavior, and so just
+ // needs to be accounted for somehow).
+ {
+ loop.set_interval_and_offset(kInterval, offset,
+ last_time + milliseconds(1));
+ EXPECT_EQ(0, loop.Iterate(last_time + milliseconds(1)));
+ }
+
+ const auto next_time = last_time + milliseconds(1) + kAllIterationsInterval;
+ EXPECT_EQ(kIterations, loop.Iterate(next_time));
+ last_time = next_time;
+ }
+}
+
+// Tests that the phased loop is correctly adjusting when the offset is
// changed to 0.
TEST_F(PhasedLoopTest, ChangingOffset) {
const auto kOffset = milliseconds(900);
diff --git a/aos/util/threaded_consumer.h b/aos/util/threaded_consumer.h
new file mode 100644
index 0000000..95ec79a
--- /dev/null
+++ b/aos/util/threaded_consumer.h
@@ -0,0 +1,102 @@
+#ifndef AOS_UTIL_THREADED_CONSUMER_H_
+#define AOS_UTIL_THREADED_CONSUMER_H_
+
+#include <functional>
+#include <optional>
+#include <thread>
+
+#include "aos/condition.h"
+#include "aos/containers/ring_buffer.h"
+#include "aos/mutex/mutex.h"
+#include "aos/realtime.h"
+
+namespace aos {
+namespace util {
+
+// This class implements a threadpool of a single worker that accepts work
+// from the main thread through a queue and executes it at a different realtime
+// priority.
+//
+// There is no mechanism to get data back to the main thread, the worker only
+// acts as a consumer. When this class is destroyed, it join()s the worker and
+// finishes all outstanding tasks.
+template <typename T, int QueueSize>
+class ThreadedConsumer {
+ public:
+ // Constructs a new ThreadedConsumer with the given consumer function to be
+ // run at the given realtime priority. If worker_priority is zero, the thread
+ // will stay at non realtime priority.
+ ThreadedConsumer(std::function<void(T)> consumer_function,
+ int worker_priority)
+ : consumer_function_(consumer_function),
+ worker_priority_(worker_priority),
+ more_tasks_(&mutex_),
+ worker_thread_([this]() { WorkerFunction(); }) {}
+
+ ~ThreadedConsumer() {
+ {
+ aos::MutexLocker locker(&mutex_);
+ quit_ = true;
+ more_tasks_.Broadcast();
+ }
+ worker_thread_.join();
+ }
+
+ // Submits another task to be processed by the worker.
+ // Returns true if successfully pushed onto the queue, and false if the queue
+ // is full.
+ bool Push(T task) {
+ aos::MutexLocker locker(&mutex_);
+
+ if (task_queue_.full()) {
+ return false;
+ }
+
+ task_queue_.Push(task);
+ more_tasks_.Broadcast();
+
+ return true;
+ }
+
+ private:
+ void WorkerFunction() {
+ if (worker_priority_ > 0) {
+ aos::SetCurrentThreadRealtimePriority(worker_priority_);
+ }
+
+ while (true) {
+ std::optional<T> task;
+
+ {
+ aos::MutexLocker locker(&mutex_);
+ while (task_queue_.empty() && !quit_) {
+ CHECK(!more_tasks_.Wait());
+ }
+
+ if (task_queue_.empty() && quit_) break;
+
+ // Pop
+ task = std::move(task_queue_[0]);
+ task_queue_.Shift();
+ }
+
+ consumer_function_(*task);
+ task.reset();
+ }
+
+ aos::UnsetCurrentThreadRealtimePriority();
+ }
+
+ std::function<void(T)> consumer_function_;
+ aos::RingBuffer<T, QueueSize> task_queue_;
+ aos::Mutex mutex_;
+ bool quit_ = false;
+ int worker_priority_;
+ aos::Condition more_tasks_;
+ std::thread worker_thread_;
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_THREADWORKER_H_
diff --git a/aos/util/threaded_consumer_test.cc b/aos/util/threaded_consumer_test.cc
new file mode 100644
index 0000000..f137108
--- /dev/null
+++ b/aos/util/threaded_consumer_test.cc
@@ -0,0 +1,144 @@
+#include "aos/util/threaded_consumer.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace util {
+
+// We expect it to be able to pass through everything we submit and recieves it
+// in the order that we submitted it. It should also be able to take in more
+// tasks than the size of the ring buffer as long as the worker doesn't get
+// behind.
+TEST(ThreadedConsumerTest, BasicFunction) {
+ std::atomic<int> counter{0};
+
+ ThreadedConsumer<int, 4> threaded_consumer(
+ [&counter](int task) {
+ LOG(INFO) << "task:" << task << " counter: " << counter;
+ counter = task;
+ },
+ 0);
+
+ for (int number : {9, 7, 1, 3, 100, 300, 42}) {
+ EXPECT_TRUE(threaded_consumer.Push(number));
+
+ // wait
+ while (counter != number) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+
+ EXPECT_EQ(counter, number);
+ }
+}
+
+// We should be able to raise the realtime priority of the worker thread, and
+// everything should work the same. It should also reset back to lower priority
+// when shutting down the worker thread.
+TEST(ThreadedConsumerTest, ElevatedPriority) {
+ std::atomic<int> counter{0};
+
+ {
+ ThreadedConsumer<int, 4> threaded_consumer(
+ [&counter](int task) {
+ CheckRealtime();
+ LOG(INFO) << "task:" << task << " counter: " << counter;
+ counter = task;
+ },
+ 20);
+
+ for (int number : {9, 7, 1, 3, 100, 300, 42}) {
+ EXPECT_TRUE(threaded_consumer.Push(number));
+
+ // wait
+ while (counter != number) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+
+ EXPECT_EQ(counter, number);
+ }
+ }
+ // TODO: Check that the worker thread's priority actually gets reset before
+ // the thread is destroyed.
+
+ CheckNotRealtime();
+}
+
+// If the worker gets behind, we shouldn't silently take in more tasks and
+// destroy old ones.
+TEST(ThreadedConsumerTest, OverflowRingBuffer) {
+ std::atomic<int> counter{0};
+ std::atomic<int> should_block{true};
+
+ ThreadedConsumer<int, 4> threaded_consumer(
+ [&counter, &should_block](int task) {
+ LOG(INFO) << "task:" << task << " counter: " << counter;
+
+ counter = task;
+
+ // prevent it from making any progress to simulate it getting behind
+ while (should_block) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+ },
+ 20);
+
+ // It consumes the 5 and then our worker blocks.
+ EXPECT_TRUE(threaded_consumer.Push(5));
+
+ // Wait for it to consume 5
+ while (counter != 5) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ };
+
+ // 4 more fills up the queue.
+ for (int number : {8, 9, 7, 1}) {
+ EXPECT_TRUE(threaded_consumer.Push(number));
+ }
+
+ // this one should overflow the buffer.
+ EXPECT_FALSE(threaded_consumer.Push(101));
+
+ // clean up, so we don't join() an infinite loop
+ should_block = false;
+}
+
+// The class should destruct gracefully and finish all of its work before
+// dissapearing.
+TEST(ThreadedConsumerTest, FinishesTasksOnQuit) {
+ std::atomic<int> counter{0};
+ std::atomic<int> should_block{true};
+
+ {
+ ThreadedConsumer<int, 4> threaded_consumer(
+ [&counter, &should_block](int task) {
+ LOG(INFO) << "task:" << task << " counter: " << counter;
+
+ counter = task;
+
+ // prevent it from making any progress to simulate it getting behind
+ while (should_block) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ }
+ },
+ 20);
+
+ // Give it some work to do
+ for (int number : {8, 9, 7, 1}) {
+ EXPECT_TRUE(threaded_consumer.Push(number));
+ }
+
+ // Wait for it to consume the first number
+ while (counter != 8) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ };
+
+ // allow it to continue
+ should_block = false;
+ }
+
+ // It should have finished all the work and gotten to the last number.
+ EXPECT_EQ(counter, 1);
+}
+
+} // namespace util
+} // namespace aos
diff --git a/frc971/control_loops/python/basic_window.py b/frc971/control_loops/python/basic_window.py
index 44e4f49..886a5d3 100755
--- a/frc971/control_loops/python/basic_window.py
+++ b/frc971/control_loops/python/basic_window.py
@@ -7,7 +7,7 @@
from gi.repository import Gdk
from gi.repository import GdkX11
import cairo
-from constants import *
+from frc971.control_loops.python.constants import *
identity = cairo.Matrix()
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 80a4a53..2fed5f0 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -3,6 +3,7 @@
from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
import numpy
+import math
import sys
from matplotlib import pylab
import glog
@@ -33,7 +34,8 @@
dt=0.00505,
controller_poles=[0.90, 0.90],
observer_poles=[0.02, 0.02],
- robot_cg_offset=0.0):
+ robot_cg_offset=0.0,
+ coefficient_of_friction=1.0):
"""Defines all constants of a drivetrain.
Args:
@@ -107,6 +109,7 @@
self.num_motors = num_motors
self.controller_poles = controller_poles
self.observer_poles = observer_poles
+ self.coefficient_of_friction = coefficient_of_friction
class Drivetrain(control_loop.ControlLoop):
@@ -533,6 +536,221 @@
kf_loop_writer.Write(kf_drivetrain_files[0], kf_drivetrain_files[1])
+def PlotDrivetrainSprint(drivetrain_params):
+ # Simulate the response of the system to a step input.
+ drivetrain = KFDrivetrain(left_low=False,
+ right_low=False,
+ drivetrain_params=drivetrain_params)
+ simulated_left_position = []
+ simulated_right_position = []
+ simulated_left_velocity = []
+ simulated_right_velocity = []
+
+ simulated_left_motor_currents = []
+ simulated_left_breaker_currents = []
+ simulated_right_motor_currents = []
+ simulated_right_breaker_currents = []
+
+ simulated_battery_heat_wattages = []
+ simulated_wattage = []
+ motor_inverter_voltages = []
+ voltage_left = []
+ voltage_right = []
+ simulated_motor_heat_wattages = []
+ simulated_motor_wattage = []
+
+ max_motor_currents = []
+ overall_currents = []
+ simulated_battery_wattage = []
+
+ # Distance in meters to call 1/2 field.
+ kSprintDistance = 8.0
+
+ vbat = 12.6
+ # Measured resistance of the battery, pd board, and breakers.
+ Rw = 0.023
+ top_speed = drivetrain.free_speed * (drivetrain.Gr +
+ drivetrain.Gl) / 2.0 * drivetrain.r
+
+ passed_distance = False
+ max_breaker_current = 0
+ heat_energy_usage = 0.0
+ for index in range(800):
+ # Current per side
+ left_traction_current = (drivetrain.mass / 2.0 *
+ drivetrain_params.coefficient_of_friction *
+ 9.81 * drivetrain.r * drivetrain.Gl /
+ drivetrain.Kt)
+ right_traction_current = (drivetrain.mass / 2.0 *
+ drivetrain_params.coefficient_of_friction *
+ 9.81 * drivetrain.r * drivetrain.Gr /
+ drivetrain.Kt)
+
+ # Detect if we've traveled over the sprint distance and report stats.
+ if (drivetrain.X[0, 0] + drivetrain.X[2, 0]) / 2.0 > kSprintDistance:
+ if not passed_distance:
+ velocity = (drivetrain.X[1, 0] + drivetrain.X[3, 0]) / 2.0
+ print("Took", index * drivetrain.dt,
+ "to pass 1/2 field, going", velocity, "m/s,",
+ velocity / 0.0254 / 12.0, "Traction limit current",
+ left_traction_current / drivetrain_params.num_motors,
+ "max breaker current", max_breaker_current, "top speed",
+ top_speed, "m/s", top_speed / 0.0254 / 12.0,
+ "fps, gear ratio", drivetrain.Gl, "heat energy",
+ heat_energy_usage)
+ passed_distance = True
+
+ bemf_left = drivetrain.X[
+ 1, 0] / drivetrain.r / drivetrain.Gl / drivetrain.Kv
+ bemf_right = drivetrain.X[
+ 3, 0] / drivetrain.r / drivetrain.Gr / drivetrain.Kv
+
+ # Max current we could push through the motors is what we would get if
+ # we short the battery through the battery resistance into the motor.
+ max_motor_current = (vbat - (bemf_left + bemf_right) / 2.0) / (
+ Rw + drivetrain.resistance / 2.0)
+
+ max_motor_currents.append(max_motor_current /
+ (drivetrain_params.num_motors * 2))
+
+ # From this current, we can compute the voltage we can apply.
+ # This is either the traction limit or the current limit.
+ max_voltage_left = bemf_left + min(
+ max_motor_current / 2,
+ left_traction_current) * drivetrain.resistance
+ max_voltage_right = bemf_right + min(
+ max_motor_current / 2,
+ right_traction_current) * drivetrain.resistance
+
+ simulated_left_position.append(drivetrain.X[0, 0])
+ simulated_left_velocity.append(drivetrain.X[1, 0])
+ simulated_right_position.append(drivetrain.X[2, 0])
+ simulated_right_velocity.append(drivetrain.X[3, 0])
+
+ U = numpy.matrix([[min(max_voltage_left, vbat)],
+ [min(max_voltage_right, vbat)]])
+
+ # Stator current
+ simulated_left_motor_current = (U[0, 0] -
+ bemf_left) / drivetrain.resistance
+ simulated_right_motor_current = (U[1, 0] -
+ bemf_right) / drivetrain.resistance
+
+ # And this gives us the power pushed into the motors.
+ power = (U[0, 0] * simulated_left_motor_current +
+ U[1, 0] * simulated_right_motor_current)
+
+ simulated_wattage.append(power)
+
+ # Solve for the voltage we'd have to supply to the input of the motor
+ # controller to generate the power required.
+ motor_inverter_voltage = (
+ vbat + numpy.sqrt(vbat * vbat - 4.0 * power * Rw)) / 2.0
+
+ overall_current = (vbat - motor_inverter_voltage) / Rw
+ overall_currents.append(overall_current)
+
+ motor_inverter_voltages.append(motor_inverter_voltage)
+
+ # Overall left and right currents at the breaker
+ simulated_left_breaker_current = (
+ simulated_left_motor_current /
+ drivetrain_params.num_motors) * U[0, 0] / motor_inverter_voltage
+ simulated_right_breaker_current = (
+ simulated_right_motor_current /
+ drivetrain_params.num_motors) * U[1, 0] / motor_inverter_voltage
+
+ simulated_left_motor_currents.append(simulated_left_motor_current /
+ drivetrain_params.num_motors)
+ simulated_left_breaker_currents.append(simulated_left_breaker_current)
+ simulated_right_motor_currents.append(simulated_right_motor_current /
+ drivetrain_params.num_motors)
+ simulated_right_breaker_currents.append(
+ simulated_right_breaker_current)
+
+ # Save out the peak battery current observed.
+ max_breaker_current = max(
+ max_breaker_current,
+ max(simulated_left_breaker_current,
+ simulated_right_breaker_current))
+
+ # Compute the heat burned in the battery
+ simulated_battery_heat_wattage = math.pow(
+ vbat - motor_inverter_voltage, 2.0) / Rw
+ simulated_battery_heat_wattages.append(simulated_battery_heat_wattage)
+
+ motor_heat_wattage = (math.pow(simulated_left_motor_current, 2.0) *
+ drivetrain.resistance +
+ math.pow(simulated_right_motor_current, 2.0) *
+ drivetrain.resistance)
+ simulated_motor_heat_wattages.append(motor_heat_wattage)
+
+ simulated_motor_wattage.append(simulated_left_motor_current * U[0, 0] +
+ simulated_right_motor_current * U[1, 0])
+
+ simulated_battery_wattage.append(vbat * overall_current)
+
+ # And then the overall energy outputted by the battery.
+ heat_energy_usage += (motor_heat_wattage +
+ simulated_battery_heat_wattage) * drivetrain.dt
+
+ voltage_left.append(U[0, 0])
+ voltage_right.append(U[1, 0])
+
+ drivetrain.Update(U)
+
+ t = [drivetrain.dt * x for x in range(len(simulated_left_position))]
+ pylab.rc('lines', linewidth=4)
+ pylab.subplot(3, 1, 1)
+ pylab.plot(t, simulated_left_position, label='left position')
+ pylab.plot(t, simulated_right_position, 'r--', label='right position')
+ pylab.plot(t, simulated_left_velocity, label='left velocity')
+ pylab.plot(t, simulated_right_velocity, label='right velocity')
+
+ pylab.suptitle('Acceleration Test\n12 Volt Step Input')
+ pylab.legend(loc='lower right')
+
+ pylab.subplot(3, 1, 2)
+
+ pylab.plot(t, simulated_left_motor_currents, label='left rotor current')
+ pylab.plot(t,
+ simulated_right_motor_currents,
+ 'r--',
+ label='right rotor current')
+ pylab.plot(t,
+ simulated_left_breaker_currents,
+ label='left breaker current')
+ pylab.plot(t,
+ simulated_right_breaker_currents,
+ 'r--',
+ label='right breaker current')
+ pylab.plot(t, motor_inverter_voltages, label='motor inverter voltage')
+ pylab.plot(t, voltage_left, label='left voltage')
+ pylab.plot(t, voltage_right, label='right voltage')
+ pylab.plot(t, max_motor_currents, label='max_currents')
+ pylab.legend(loc='lower right')
+
+ wattage_axis = pylab.subplot(3, 1, 3)
+ wattage_axis.plot(t, simulated_wattage, label='wattage')
+ wattage_axis.plot(t,
+ simulated_battery_heat_wattages,
+ label='battery wattage')
+ wattage_axis.plot(t,
+ simulated_motor_heat_wattages,
+ label='motor heat wattage')
+ wattage_axis.plot(t, simulated_motor_wattage, label='motor wattage')
+ wattage_axis.plot(t, simulated_battery_wattage, label='overall wattage')
+ pylab.legend(loc='upper left')
+ overall_current_axis = wattage_axis.twinx()
+ overall_current_axis.plot(t, overall_currents, 'c--', label='current')
+
+ pylab.legend(loc='lower right')
+
+ pylab.suptitle('Acceleration Test\n12 Volt Step Input\n%f fps' %
+ (top_speed / 0.0254 / 12.0, ))
+ pylab.show()
+
+
def PlotDrivetrainMotions(drivetrain_params):
# Test out the voltage error.
drivetrain = KFDrivetrain(left_low=False,
diff --git a/frc971/rockpi/build_rootfs.sh b/frc971/rockpi/build_rootfs.sh
index 4bffe67..74d4dc1 100755
--- a/frc971/rockpi/build_rootfs.sh
+++ b/frc971/rockpi/build_rootfs.sh
@@ -186,7 +186,7 @@
target "apt-get -y install -t bullseye-backports bpfcc-tools"
-target "apt-get install -y sudo openssh-server python3 bash-completion git v4l-utils cpufrequtils pmount rsync vim-nox chrony libopencv-calib3d4.5 libopencv-contrib4.5 libopencv-core4.5 libopencv-features2d4.5 libopencv-flann4.5 libopencv-highgui4.5 libopencv-imgcodecs4.5 libopencv-imgproc4.5 libopencv-ml4.5 libopencv-objdetect4.5 libopencv-photo4.5 libopencv-shape4.5 libopencv-stitching4.5 libopencv-superres4.5 libopencv-video4.5 libopencv-videoio4.5 libopencv-videostab4.5 libopencv-viz4.5 libnice10 pmount libnice-dev feh libgstreamer1.0-0 libgstreamer-plugins-base1.0-0 libgstreamer-plugins-bad1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-nice usbutils locales trace-cmd clinfo"
+target "apt-get install -y sudo openssh-server python3 bash-completion git v4l-utils cpufrequtils pmount rsync vim-nox chrony libopencv-calib3d4.5 libopencv-contrib4.5 libopencv-core4.5 libopencv-features2d4.5 libopencv-flann4.5 libopencv-highgui4.5 libopencv-imgcodecs4.5 libopencv-imgproc4.5 libopencv-ml4.5 libopencv-objdetect4.5 libopencv-photo4.5 libopencv-shape4.5 libopencv-stitching4.5 libopencv-superres4.5 libopencv-video4.5 libopencv-videoio4.5 libopencv-videostab4.5 libopencv-viz4.5 libnice10 pmount libnice-dev feh libgstreamer1.0-0 libgstreamer-plugins-base1.0-0 libgstreamer-plugins-bad1.0-0 gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-nice usbutils locales trace-cmd clinfo jq"
target "cd /tmp && wget https://software.frc971.org/Build-Dependencies/libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb && sudo dpkg -i libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb && rm libmali-midgard-t86x-r14p0-x11_1.9-1_arm64.deb"
target "apt-get clean"
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 5d20aff..642ca0f 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -77,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",
],
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 864c5e7..f12a6a5 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -27,7 +27,6 @@
"The mininum number of aruco targets in charuco board required to match.");
DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
-DEFINE_uint32(age, 100, "Age to start dropping frames at.");
DEFINE_uint32(disable_delay, 100, "Time after an issue to disable tracing at.");
DECLARE_bool(enable_ftrace);
@@ -94,8 +93,8 @@
ImageCallback::ImageCallback(
aos::EventLoop *event_loop, std::string_view channel,
- std::function<void(cv::Mat, monotonic_clock::time_point)> &&handle_image_fn)
-
+ std::function<void(cv::Mat, monotonic_clock::time_point)> &&handle_image_fn,
+ monotonic_clock::duration max_age)
: event_loop_(event_loop),
server_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
@@ -106,7 +105,8 @@
->source_node()
->string_view())),
handle_image_(std::move(handle_image_fn)),
- timer_fn_(event_loop->AddTimer([this]() { DisableTracing(); })) {
+ timer_fn_(event_loop->AddTimer([this]() { DisableTracing(); })),
+ max_age_(max_age) {
event_loop_->MakeWatcher(channel, [this](const CameraImage &image) {
const monotonic_clock::time_point eof_source_node =
monotonic_clock::time_point(
@@ -145,7 +145,7 @@
const monotonic_clock::duration age = event_loop_->monotonic_now() - eof;
const double age_double =
std::chrono::duration_cast<std::chrono::duration<double>>(age).count();
- if (age > std::chrono::milliseconds(FLAGS_age)) {
+ if (age > max_age_) {
if (FLAGS_enable_ftrace) {
ftrace_.FormatMessage("Too late receiving image, age: %f\n",
age_double);
@@ -230,12 +230,6 @@
marker_length_ = 0.15;
square_length_ = 0.1651;
dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250);
- } else if (target_type_ == TargetType::kAprilTag) {
- // Tag will be 6 in (15.24 cm) according to
- // https://www.firstinspires.org/robotics/frc/blog/2022-2023-approved-devices-rules-preview-and-vision-target-update
- square_length_ = 0.1524;
- dictionary_ =
- cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_16h5);
} else {
// Bail out if it's not a supported target
LOG(FATAL) << "Target type undefined: "
@@ -435,9 +429,8 @@
<< ", not enough marker IDs for charuco board, got "
<< marker_ids.size() << ", needed " << FLAGS_min_charucos;
}
- } else if (target_type_ == TargetType::kAprilTag ||
- target_type_ == TargetType::kAruco) {
- // estimate pose for april tags doesn't return valid, so marking true
+ } else if (target_type_ == TargetType::kAruco) {
+ // estimate pose for arucos doesn't return valid, so marking true
valid = true;
std::vector<cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(marker_corners, square_length_,
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index e296071..984cef6 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -55,13 +55,15 @@
BGR = 1,
GRAYSCALE = 2,
};
- ImageCallback(aos::EventLoop *event_loop, std::string_view channel,
- std::function<void(cv::Mat, aos::monotonic_clock::time_point)>
- &&handle_image_fn);
- void set_format(Format format) {
- format_ = format;
- }
+ // `max_age` is the age to start dropping frames at
+ ImageCallback(
+ aos::EventLoop *event_loop, std::string_view channel,
+ std::function<void(cv::Mat, aos::monotonic_clock::time_point)>
+ &&handle_image_fn,
+ aos::monotonic_clock::duration max_age = std::chrono::milliseconds(100));
+
+ void set_format(Format format) { format_ = format; }
private:
void DisableTracing();
@@ -77,14 +79,15 @@
aos::Ftrace ftrace_;
Format format_ = Format::BGR;
+
+ aos::monotonic_clock::duration max_age_;
};
// Types of targets that a CharucoExtractor can detect in images
enum class TargetType : uint8_t {
- kAprilTag = 0,
- kAruco = 1,
- kCharuco = 2,
- kCharucoDiamond = 3
+ kAruco = 0,
+ kCharuco = 1,
+ kCharucoDiamond = 2
};
// Class which calls a callback each time an image arrives with the information
diff --git a/frc971/vision/target_map.fbs b/frc971/vision/target_map.fbs
index 38bab6d..cee2b07 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -1,29 +1,44 @@
namespace frc971.vision;
+table Position {
+ x:double (id: 0);
+ y:double (id: 1);
+ z:double (id: 2);
+}
+
+table Quaternion {
+ w:double (id: 0);
+ x:double (id: 1);
+ y:double (id: 2);
+ z:double (id: 3);
+}
+
// Represents 3d pose of an april tag on the field.
table TargetPoseFbs {
// AprilTag ID of this target
id:uint64 (id: 0);
- // Pose of target relative to field origin.
- // To get the pose of the target in the field frame, do:
- // Translation3d(x, y, z) *
- // (AngleAxisd(yaw) * AngleAxisd(pitch) * AngleAxisd(roll))
- x:double (id: 1);
- y:double (id: 2);
- z:double (id: 3);
-
- // Orientation of the target.
- roll:double (id: 4);
- pitch:double (id: 5);
- yaw:double (id: 6);
+ // Pose of target relative to either the field origin or robot.
+ // To get the pose of the target, do:
+ // Translation3d(position.x(), position.y(), position.z()) *
+ // Quaterniond(orientation.w(), orientation.x(), orientation.y(), orientation.z())
+ position:Position (id: 1);
+ orientation:Quaternion (id: 2);
}
// Map of all target poses on a field.
-// This would be solved for by TargetMapper
+// There are two possible uses for this:
+// 1. Static april tag poses on the field solved for by TargetMapper.
+// 2. List of detected april poses relative to the robot.
table TargetMap {
target_poses:[TargetPoseFbs] (id: 0);
- // Unique name of the field
+ // Unique name of the field (for use case 1.)
field_name:string (id: 1);
-}
\ No newline at end of file
+
+ // End-of-frame timestamp for the frame with tag detections.
+ // (for use case 2.).
+ monotonic_timestamp_ns:int64 (id: 2);
+}
+
+root_type TargetMap;
diff --git a/frc971/vision/target_map.json b/frc971/vision/target_map.json
index 5f256fa..3f6eb54 100644
--- a/frc971/vision/target_map.json
+++ b/frc971/vision/target_map.json
@@ -2,75 +2,123 @@
"target_poses": [
{
"id": 1,
- "x": 7.244,
- "y": -2.938,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.244,
+ "y": -2.938,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 2,
- "x": 7.244,
- "y": -1.262,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.244,
+ "y": -1.262,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 3,
- "x": 7.244,
- "y": 0.414,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.244,
+ "y": 0.414,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 4,
- "x": 7.909,
- "y": 2.740,
- "z": 0.695,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.909,
+ "y": 2.740,
+ "z": 0.695
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 5,
- "x": -7.908,
- "y": 2.740,
- "z": 0.695,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.908,
+ "y": 2.740,
+ "z": 0.695
+ },
+ "orientation": {
+ /* yaw of 0 */
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
},
{
"id": 6,
- "x": -7.243,
- "y": 0.414,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.243,
+ "y": 0.414,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
},
{
"id": 7,
- "x": -7.243,
- "y": -1.262,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.243,
+ "y": -1.262,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
},
{
"id": 8,
- "x": -7.243,
- "y": -2.938,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.243,
+ "y": -2.938,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
}
]
}
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 5ce62a0..b3049e4 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -66,6 +66,32 @@
return Eigen::Vector3d(roll, pitch, yaw);
}
+flatbuffers::Offset<TargetPoseFbs> PoseUtils::TargetPoseToFbs(
+ const TargetMapper::TargetPose &target_pose,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const auto position_offset =
+ CreatePosition(*fbb, target_pose.pose.p(0), target_pose.pose.p(1),
+ target_pose.pose.p(2));
+ const auto orientation_offset =
+ CreateQuaternion(*fbb, target_pose.pose.q.w(), target_pose.pose.q.x(),
+ target_pose.pose.q.y(), target_pose.pose.q.z());
+ return CreateTargetPoseFbs(*fbb, target_pose.id, position_offset,
+ orientation_offset);
+}
+
+TargetMapper::TargetPose PoseUtils::TargetPoseFromFbs(
+ const TargetPoseFbs &target_pose_fbs) {
+ return {.id = static_cast<TargetMapper::TargetId>(target_pose_fbs.id()),
+ .pose = ceres::examples::Pose3d{
+ Eigen::Vector3d(target_pose_fbs.position()->x(),
+ target_pose_fbs.position()->y(),
+ target_pose_fbs.position()->z()),
+ Eigen::Quaterniond(target_pose_fbs.orientation()->w(),
+ target_pose_fbs.orientation()->x(),
+ target_pose_fbs.orientation()->y(),
+ target_pose_fbs.orientation()->z())}};
+}
+
ceres::examples::VectorOfConstraints DataAdapter::MatchTargetDetections(
const std::vector<DataAdapter::TimestampedDetection>
×tamped_target_detections,
@@ -177,12 +203,8 @@
aos::FlatbufferDetachedBuffer<TargetMap> target_map =
aos::JsonFileToFlatbuffer<TargetMap>(target_poses_path);
for (const auto *target_pose_fbs : *target_map.message().target_poses()) {
- target_poses_[target_pose_fbs->id()] = ceres::examples::Pose3d{
- Eigen::Vector3d(target_pose_fbs->x(), target_pose_fbs->y(),
- target_pose_fbs->z()),
- PoseUtils::EulerAnglesToQuaternion(
- Eigen::Vector3d(target_pose_fbs->roll(), target_pose_fbs->pitch(),
- target_pose_fbs->yaw()))};
+ target_poses_[target_pose_fbs->id()] =
+ PoseUtils::TargetPoseFromFbs(*target_pose_fbs).pose;
}
}
@@ -215,7 +237,7 @@
return;
}
- ceres::LossFunction *loss_function = NULL;
+ ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0);
ceres::LocalParameterization *quaternion_local_parameterization =
new ceres::EigenQuaternionParameterization;
@@ -253,12 +275,12 @@
}
// The pose graph optimization problem has six DOFs that are not fully
- // constrained. This is typically referred to as gauge freedom. You can apply
- // a rigid body transformation to all the nodes and the optimization problem
- // will still have the exact same cost. The Levenberg-Marquardt algorithm has
- // internal damping which mitigates this issue, but it is better to properly
- // constrain the gauge freedom. This can be done by setting one of the poses
- // as constant so the optimizer cannot change it.
+ // constrained. This is typically referred to as gauge freedom. You can
+ // apply a rigid body transformation to all the nodes and the optimization
+ // problem will still have the exact same cost. The Levenberg-Marquardt
+ // algorithm has internal damping which mitigates this issue, but it is
+ // better to properly constrain the gauge freedom. This can be done by
+ // setting one of the poses as constant so the optimizer cannot change it.
ceres::examples::MapOfPoses::iterator pose_start_iter = poses->begin();
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
@@ -306,19 +328,8 @@
// Convert poses to flatbuffers
std::vector<flatbuffers::Offset<TargetPoseFbs>> target_poses_fbs;
for (const auto &[id, pose] : target_poses_) {
- TargetPoseFbs::Builder target_pose_builder(fbb);
- target_pose_builder.add_id(id);
-
- target_pose_builder.add_x(pose.p(0));
- target_pose_builder.add_y(pose.p(1));
- target_pose_builder.add_z(pose.p(2));
-
- auto rpy = PoseUtils::QuaternionToEulerAngles(pose.q);
- target_pose_builder.add_roll(rpy.x());
- target_pose_builder.add_pitch(rpy.y());
- target_pose_builder.add_yaw(rpy.z());
-
- target_poses_fbs.emplace_back(target_pose_builder.Finish());
+ target_poses_fbs.emplace_back(
+ PoseUtils::TargetPoseToFbs(TargetPose{.id = id, .pose = pose}, &fbb));
}
const auto field_name_offset = fbb.CreateString(field_name);
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 7e2c323..7701fd1 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -89,6 +89,14 @@
static Eigen::Vector3d QuaternionToEulerAngles(const Eigen::Quaterniond &q);
// Converts a 3d rotation matrix into a rotation with roll, pitch, and yaw
static Eigen::Vector3d RotationMatrixToEulerAngles(const Eigen::Matrix3d &R);
+
+ // Builds a TargetPoseFbs from a TargetPose
+ static flatbuffers::Offset<TargetPoseFbs> TargetPoseToFbs(
+ const TargetMapper::TargetPose &target_pose,
+ flatbuffers::FlatBufferBuilder *fbb);
+ // Converts a TargetPoseFbs to a TargetPose
+ static TargetMapper::TargetPose TargetPoseFromFbs(
+ const TargetPoseFbs &target_pose_fbs);
};
// Transforms robot position and target detection data into target constraints
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index 9440813..271f787 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -408,15 +408,9 @@
std::vector<TargetMapper::TargetPose> actual_target_poses;
ceres::examples::MapOfPoses target_poses;
- for (auto *target_pose_fbs : *target_map_fbs.message().target_poses()) {
- auto target_pose = TargetMapper::TargetPose{
- static_cast<int>(target_pose_fbs->id()),
- ceres::examples::Pose3d{
- Eigen::Vector3d(target_pose_fbs->x(), target_pose_fbs->y(),
- target_pose_fbs->z()),
- PoseUtils::EulerAnglesToQuaternion(Eigen::Vector3d(
- target_pose_fbs->roll(), target_pose_fbs->pitch(),
- target_pose_fbs->yaw()))}};
+ for (const auto *target_pose_fbs : *target_map_fbs.message().target_poses()) {
+ const TargetMapper::TargetPose target_pose =
+ PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
actual_target_poses.emplace_back(target_pose);
target_poses[target_pose.id] = target_pose.pose;
}
diff --git a/frc971/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
index a6bcb4d..7c0546f 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,21 @@
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); }, 20) {
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..31c0888 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,15 @@
private:
void OnImageReady();
+ void MarkBufferToBeEnqueued(int buffer) override;
+
int ImageSensorIoctl(unsigned long number, void *arg);
aos::internal::EPoll *epoll_;
aos::ScopedFD image_sensor_fd_;
+
+ aos::util::ThreadedConsumer<int, kNumberBuffers> buffer_requeuer_;
};
} // namespace vision
diff --git a/third_party/apriltag/BUILD b/third_party/apriltag/BUILD
new file mode 100644
index 0000000..b11e34f
--- /dev/null
+++ b/third_party/apriltag/BUILD
@@ -0,0 +1,93 @@
+load("@//tools/build_rules:select.bzl", "compiler_select")
+
+cc_library(
+ name = "apriltag",
+ srcs = [
+ "apriltag.c",
+ "apriltag_pose.c",
+ "apriltag_quad_thresh.c",
+ "common/g2d.c",
+ "common/getopt.c",
+ "common/homography.c",
+ "common/image_u8.c",
+ "common/image_u8x3.c",
+ "common/image_u8x4.c",
+ "common/matd.c",
+ "common/pam.c",
+ "common/pjpeg.c",
+ "common/pjpeg-idct.c",
+ "common/pnm.c",
+ "common/string_util.c",
+ "common/svd22.c",
+ "common/time_util.c",
+ "common/unionfind.c",
+ "common/workerpool.c",
+ "common/zarray.c",
+ "common/zhash.c",
+ "common/zmaxheap.c",
+ "tag16h5.c",
+ "tag25h9.c",
+ "tag36h11.c",
+ "tagCircle21h7.c",
+ "tagCircle49h12.c",
+ "tagCustom48h12.c",
+ "tagStandard41h12.c",
+ "tagStandard52h13.c",
+ ],
+ hdrs = [
+ "apriltag.h",
+ "apriltag_math.h",
+ "apriltag_pose.h",
+ "common/debug_print.h",
+ "common/g2d.h",
+ "common/getopt.h",
+ "common/homography.h",
+ "common/image_types.h",
+ "common/image_u8.h",
+ "common/image_u8x3.h",
+ "common/image_u8x4.h",
+ "common/matd.h",
+ "common/math_util.h",
+ "common/pam.h",
+ "common/pjpeg.h",
+ "common/pnm.h",
+ "common/postscript_utils.h",
+ "common/pthreads_cross.h",
+ "common/string_util.h",
+ "common/svd22.h",
+ "common/time_util.h",
+ "common/timeprofile.h",
+ "common/unionfind.h",
+ "common/workerpool.h",
+ "common/zarray.h",
+ "common/zhash.h",
+ "common/zmaxheap.h",
+ "tag16h5.h",
+ "tag25h9.h",
+ "tag36h11.h",
+ "tagCircle21h7.h",
+ "tagCircle49h12.h",
+ "tagCustom48h12.h",
+ "tagStandard41h12.h",
+ "tagStandard52h13.h",
+ ],
+ copts = compiler_select({
+ "clang": [
+ "-Wno-cast-align",
+ "-Wno-incompatible-pointer-types-discards-qualifiers",
+ ],
+ "gcc": [
+ "-Wno-discarded-qualifiers",
+ "-Wno-shift-negative-value",
+ ],
+ }) + [
+ "-Wno-sign-compare",
+ "-Wno-cast-qual",
+ "-Wno-unused-parameter",
+ "-Wno-unused-variable",
+ "-Wno-format-nonliteral",
+ ],
+ includes = ["."],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/third_party/apriltag/common/workerpool.c b/third_party/apriltag/common/workerpool.c
index a0170ef..29eccfc 100644
--- a/third_party/apriltag/common/workerpool.c
+++ b/third_party/apriltag/common/workerpool.c
@@ -41,20 +41,6 @@
#include "workerpool.h"
#include "debug_print.h"
-struct workerpool {
- int nthreads;
- zarray_t *tasks;
- int taskspos;
-
- pthread_t *threads;
- int *status;
-
- pthread_mutex_t mutex;
- pthread_cond_t startcond; // used to signal the availability of work
- pthread_cond_t endcond; // used to signal completion of all work
-
- int end_count; // how many threads are done?
-};
struct task
{
diff --git a/third_party/apriltag/common/workerpool.h b/third_party/apriltag/common/workerpool.h
index 2c32ab1..a233b5b 100644
--- a/third_party/apriltag/common/workerpool.h
+++ b/third_party/apriltag/common/workerpool.h
@@ -31,6 +31,21 @@
typedef struct workerpool workerpool_t;
+struct workerpool {
+ int nthreads;
+ zarray_t *tasks;
+ int taskspos;
+
+ pthread_t *threads;
+ int *status;
+
+ pthread_mutex_t mutex;
+ pthread_cond_t startcond; // used to signal the availability of work
+ pthread_cond_t endcond; // used to signal completion of all work
+
+ int end_count; // how many threads are done?
+};
+
// as a special case, if nthreads==1, no additional threads are
// created, and workerpool_run will run synchronously.
workerpool_t *workerpool_create(int nthreads);
@@ -41,7 +56,8 @@
// runs all added tasks, waits for them to complete.
void workerpool_run(workerpool_t *wp);
-// same as workerpool_run, except always single threaded. (mostly for debugging).
+// same as workerpool_run, except always single threaded. (mostly for
+// debugging).
void workerpool_run_single(workerpool_t *wp);
int workerpool_get_nthreads(workerpool_t *wp);
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 6145452..d5ed54f 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -53,7 +53,8 @@
[this](cv::Mat rgb_image,
const aos::monotonic_clock::time_point eof) {
charuco_extractor_.HandleImage(rgb_image, eof);
- }) {
+ },
+ std::chrono::milliseconds(5)) {
CHECK(pi_number_) << ": Invalid pi number " << pi
<< ", failed to parse pi number";
std::regex re{"^[0-9][0-9]-[0-9][0-9]"};
diff --git a/y2022/vision/calibrate_extrinsics.cc b/y2022/vision/calibrate_extrinsics.cc
index 9b6c428..04b24ea 100644
--- a/y2022/vision/calibrate_extrinsics.cc
+++ b/y2022/vision/calibrate_extrinsics.cc
@@ -20,7 +20,7 @@
DEFINE_bool(plot, false, "Whether to plot the resulting data.");
DEFINE_bool(turret, true, "If true, the camera is on the turret");
DEFINE_string(target_type, "charuco",
- "Type of target: april_tag|aruco|charuco|charuco_diamond");
+ "Type of target: aruco|charuco|charuco_diamond");
DEFINE_string(image_channel, "/camera", "Channel to listen for images on");
DEFINE_string(output_logs, "/tmp/calibration/",
"Output folder for visualization logs.");
@@ -86,9 +86,7 @@
logger.StartLoggingOnRun(FLAGS_output_logs);
TargetType target_type = TargetType::kCharuco;
- if (FLAGS_target_type == "april_tag") {
- target_type = TargetType::kAprilTag;
- } else if (FLAGS_target_type == "aruco") {
+ if (FLAGS_target_type == "aruco") {
target_type = TargetType::kAruco;
} else if (FLAGS_target_type == "charuco") {
target_type = TargetType::kCharuco;
@@ -96,7 +94,7 @@
target_type = TargetType::kCharucoDiamond;
} else {
LOG(FATAL) << "Unknown target type: " << FLAGS_target_type
- << ", expected: april_tag|aruco|charuco|charuco_diamond";
+ << ", expected: aruco|charuco|charuco_diamond";
}
// Now, hook Calibration up to everything.
diff --git a/y2023/BUILD b/y2023/BUILD
index 0879de7..0c34ce8 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -7,7 +7,7 @@
binaries = [
"//y2020/vision:calibration",
"//y2023/vision:viewer",
- "//y2022/localizer:imu_main",
+ "//y2023/vision:aprilrobotics",
"//y2022/localizer:localizer_main",
"//aos/network:web_proxy_main",
"//aos/events/logging:log_cat",
@@ -16,8 +16,6 @@
":aos_config",
":message_bridge_client.sh",
"//y2022/www:www_files",
- "@ctre_phoenix_api_cpp_athena//:shared_libraries",
- "@ctre_phoenix_cci_athena//:shared_libraries",
],
dirs = [
"//y2023/www:www_files",
@@ -29,11 +27,6 @@
"//aos/starter:irq_affinity",
"//y2023/vision:camera_reader",
"//aos/events/logging:logger_main",
- ":joystick_reader",
- "//y2023/autonomous:binaries",
- "//y2023/control_loops/drivetrain:drivetrain",
- "//y2023/control_loops/drivetrain:trajectory_generator",
- "//y2023/control_loops/superstructure:superstructure",
],
target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
target_type = "pi",
@@ -48,6 +41,7 @@
"//aos/network:timestamp_fbs",
"//frc971/input:robot_state_fbs",
"//frc971/vision:vision_fbs",
+ "//frc971/vision:target_map_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -73,6 +67,7 @@
"//aos/network:remote_message_fbs",
"//y2022/localizer:localizer_output_fbs",
"//frc971/vision:calibration_fbs",
+ "//frc971/vision:target_map_fbs",
"//frc971/vision:vision_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
@@ -102,6 +97,7 @@
"//y2022/localizer:localizer_status_fbs",
"//y2022/localizer:localizer_output_fbs",
"//y2022/localizer:localizer_visualization_fbs",
+ "//frc971/vision:target_map_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -121,6 +117,7 @@
"//aos/network:remote_message_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:vision_fbs",
+ "//frc971/vision:target_map_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
diff --git a/y2023/control_loops/python/BUILD b/y2023/control_loops/python/BUILD
index b024676..a2ea6d8 100644
--- a/y2023/control_loops/python/BUILD
+++ b/y2023/control_loops/python/BUILD
@@ -31,6 +31,24 @@
],
)
+py_binary(
+ name = "graph_edit",
+ srcs = [
+ "graph_edit.py",
+ "graph_generate.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..7b6179c
--- /dev/null
+++ b/y2023/control_loops/python/graph_edit.py
@@ -0,0 +1,495 @@
+#!/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
+import graph_generate
+from graph_generate import XYSegment, AngleSegment, to_theta, to_xy, alpha_blend
+from graph_generate import back_to_xy_loop, subdivide_theta, to_theta_loop
+from graph_generate import l1, l2, joint_center
+
+from frc971.control_loops.python.basic_window import OverrideMatrix, identity, quit_main_loop, set_color
+
+import shapely
+from shapely.geometry import Polygon
+
+
+def px(cr):
+ return OverrideMatrix(cr, identity)
+
+
+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
+
+
+# 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()
+
+
+# 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_button_press(self, event):
+ pass
+
+ 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 not self.theta_version:
+ # 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()
+ else:
+ # 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()
+
+ if self.theta_version:
+ 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()
+
+ if self.theta_version:
+ set_color(cr, Color(0.5, 0.5, 1.0))
+ draw_lines(cr, lines_theta)
+ else:
+ 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()
+
+ if not self.theta_version:
+ 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)
+
+ if self.theta_version:
+ 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)
+
+ 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_generate.segments
+basic_window.RunApp()
diff --git a/y2023/control_loops/python/graph_generate.py b/y2023/control_loops/python/graph_generate.py
new file mode 100644
index 0000000..046b9dd
--- /dev/null
+++ b/y2023/control_loops/python/graph_generate.py
@@ -0,0 +1,798 @@
+import numpy
+
+# 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
+
+
+# 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))
+
+
+# Draw a list of lines to a cairo context.
+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])
+
+
+max_dist = 0.01
+max_dist_theta = numpy.pi / 64
+xy_end_circle_size = 0.01
+theta_end_circle_size = 0.07
+
+
+# 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
+
+
+# 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)
+ ]
+
+
+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)
+
+
+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)
+ ]
+
+
+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])
+
+
+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)
+ ]
+
+
+tall_box_x = 0.411
+tall_box_y = 0.125
+
+short_box_x = 0.431
+short_box_y = 0.082
+
+ready_above_box = to_theta_with_circular_index(tall_box_x,
+ tall_box_y + 0.08,
+ circular_index=-1)
+tall_box_grab = to_theta_with_circular_index(tall_box_x,
+ tall_box_y,
+ circular_index=-1)
+short_box_grab = to_theta_with_circular_index(short_box_x,
+ short_box_y,
+ circular_index=-1)
+
+# TODO(austin): Drive the front/back off the same numbers a bit better.
+front_high_box = to_theta_with_circular_index(0.378, 2.46, circular_index=-1)
+front_middle3_box = to_theta_with_circular_index(0.700,
+ 2.125,
+ circular_index=-1.000000)
+front_middle2_box = to_theta_with_circular_index(0.700,
+ 2.268,
+ circular_index=-1)
+front_middle1_box = to_theta_with_circular_index(0.800,
+ 1.915,
+ circular_index=-1)
+front_low_box = to_theta_with_circular_index(0.87, 1.572, circular_index=-1)
+back_high_box = to_theta_with_circular_index(-0.75, 2.48, circular_index=0)
+back_middle2_box = to_theta_with_circular_index(-0.700, 2.27, circular_index=0)
+back_middle1_box = to_theta_with_circular_index(-0.800, 1.93, circular_index=0)
+back_low_box = to_theta_with_circular_index(-0.87, 1.64, circular_index=0)
+
+back_extra_low_box = to_theta_with_circular_index(-0.87,
+ 1.52,
+ circular_index=0)
+
+front_switch = to_theta_with_circular_index(0.88, 0.967, circular_index=-1)
+back_switch = to_theta_with_circular_index(-0.88, 0.967, circular_index=-2)
+
+neutral = to_theta_with_circular_index(0.0, 0.33, circular_index=-1)
+
+up = to_theta_with_circular_index(0.0, 2.547, circular_index=-1)
+
+front_switch_auto = to_theta_with_circular_index(0.750,
+ 2.20,
+ circular_index=-1.000000)
+
+duck = numpy.array([numpy.pi / 2.0 - 0.92, numpy.pi / 2.0 - 4.26])
+
+starting = numpy.array([numpy.pi / 2.0 - 0.593329, numpy.pi / 2.0 - 3.749631])
+vertical_starting = numpy.array([numpy.pi / 2.0, -numpy.pi / 2.0])
+
+self_hang = numpy.array([numpy.pi / 2.0 - 0.191611, numpy.pi / 2.0])
+partner_hang = numpy.array([numpy.pi / 2.0 - (-0.30), numpy.pi / 2.0])
+
+above_hang = numpy.array([numpy.pi / 2.0 - 0.14, numpy.pi / 2.0 - (-0.165)])
+below_hang = numpy.array([numpy.pi / 2.0 - 0.39, numpy.pi / 2.0 - (-0.517)])
+
+up_c1 = to_theta((0.63, 1.17), circular_index=-1)
+up_c2 = to_theta((0.65, 1.62), circular_index=-1)
+
+front_high_box_c1 = to_theta((0.63, 1.04), circular_index=-1)
+front_high_box_c2 = to_theta((0.50, 1.60), circular_index=-1)
+
+front_middle2_box_c1 = to_theta((0.41, 0.83), circular_index=-1)
+front_middle2_box_c2 = to_theta((0.52, 1.30), circular_index=-1)
+
+front_middle1_box_c1 = to_theta((0.34, 0.82), circular_index=-1)
+front_middle1_box_c2 = to_theta((0.48, 1.15), circular_index=-1)
+
+#c1: (1.421433, -1.070254)
+#c2: (1.434384, -1.057803
+ready_above_box_c1 = numpy.array([1.480802, -1.081218])
+ready_above_box_c2 = numpy.array([1.391449, -1.060331])
+
+front_switch_c1 = numpy.array([1.903841, -0.622351])
+front_switch_c2 = numpy.array([1.903841, -0.622351])
+
+
+sparse_front_points = [
+ (front_high_box, "FrontHighBox"),
+ (front_middle2_box, "FrontMiddle2Box"),
+ (front_middle3_box, "FrontMiddle3Box"),
+ (front_middle1_box, "FrontMiddle1Box"),
+ (front_low_box, "FrontLowBox"),
+ (front_switch, "FrontSwitch"),
+] # yapf: disable
+
+sparse_back_points = [
+ (back_high_box, "BackHighBox"),
+ (back_middle2_box, "BackMiddle2Box"),
+ (back_middle1_box, "BackMiddle1Box"),
+ (back_low_box, "BackLowBox"),
+ (back_extra_low_box, "BackExtraLowBox"),
+] # yapf: disable
+
+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
+
+
+front_points, front_paths = expand_points(sparse_front_points, 0.06)
+back_points, back_paths = expand_points(sparse_back_points, 0.06)
+
+points = [(ready_above_box, "ReadyAboveBox"),
+ (tall_box_grab, "TallBoxGrab"),
+ (short_box_grab, "ShortBoxGrab"),
+ (back_switch, "BackSwitch"),
+ (neutral, "Neutral"),
+ (up, "Up"),
+ (above_hang, "AboveHang"),
+ (below_hang, "BelowHang"),
+ (self_hang, "SelfHang"),
+ (partner_hang, "PartnerHang"),
+ (front_switch_auto, "FrontSwitchAuto"),
+ (starting, "Starting"),
+ (duck, "Duck"),
+ (vertical_starting, "VerticalStarting"),
+] + front_points + back_points # yapf: disable
+
+duck_c1 = numpy.array([1.337111, -1.721008])
+duck_c2 = numpy.array([1.283701, -1.795519])
+
+ready_to_up_c1 = numpy.array([1.792962, 0.198329])
+ready_to_up_c2 = numpy.array([1.792962, 0.198329])
+
+front_switch_auto_c1 = numpy.array([1.792857, -0.372768])
+front_switch_auto_c2 = numpy.array([1.861885, -0.273664])
+
+# We need to define critical points so we can create paths connecting them.
+# TODO(austin): Attach velocities to the slow ones.
+ready_to_back_low_c1 = numpy.array([2.524325, 0.046417])
+
+neutral_to_back_low_c1 = numpy.array([2.381942, -0.070220])
+
+tall_to_back_low_c1 = numpy.array([2.603918, 0.088298])
+tall_to_back_low_c2 = numpy.array([1.605624, 1.003434])
+
+tall_to_back_high_c2 = numpy.array([1.508610, 0.946147])
+
+# If true, only plot the first named segment
+isolate = False
+
+long_alpha_unitizer = numpy.matrix([[1.0 / 17.0, 0.0], [0.0, 1.0 / 17.0]])
+
+neutral_to_back_c1 = numpy.array([0.702527, -2.618276])
+neutral_to_back_c2 = numpy.array([0.526914, -3.109691])
+
+named_segments = [
+ ThetaSplineSegment(neutral, neutral_to_back_c1, neutral_to_back_c2,
+ back_switch, "BackSwitch"),
+ ThetaSplineSegment(neutral,
+ neutral_to_back_low_c1,
+ tall_to_back_high_c2,
+ back_high_box,
+ "NeutralBoxToHigh",
+ alpha_unitizer=long_alpha_unitizer),
+ ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_high_c2,
+ back_middle2_box, "NeutralBoxToMiddle2",
+ long_alpha_unitizer),
+ ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
+ back_middle1_box, "NeutralBoxToMiddle1",
+ long_alpha_unitizer),
+ ThetaSplineSegment(neutral, neutral_to_back_low_c1, tall_to_back_low_c2,
+ back_low_box, "NeutralBoxToLow", long_alpha_unitizer),
+ ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+ tall_to_back_high_c2, back_high_box, "ReadyBoxToHigh",
+ long_alpha_unitizer),
+ ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+ tall_to_back_high_c2, back_middle2_box,
+ "ReadyBoxToMiddle2", long_alpha_unitizer),
+ ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+ tall_to_back_low_c2, back_middle1_box,
+ "ReadyBoxToMiddle1", long_alpha_unitizer),
+ ThetaSplineSegment(ready_above_box, ready_to_back_low_c1,
+ tall_to_back_low_c2, back_low_box, "ReadyBoxToLow",
+ long_alpha_unitizer),
+ ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+ tall_to_back_high_c2, back_high_box, "ShortBoxToHigh",
+ long_alpha_unitizer),
+ ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+ tall_to_back_high_c2, back_middle2_box,
+ "ShortBoxToMiddle2", long_alpha_unitizer),
+ ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+ tall_to_back_low_c2, back_middle1_box,
+ "ShortBoxToMiddle1", long_alpha_unitizer),
+ ThetaSplineSegment(short_box_grab, tall_to_back_low_c1,
+ tall_to_back_low_c2, back_low_box, "ShortBoxToLow",
+ long_alpha_unitizer),
+ ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
+ tall_to_back_high_c2, back_high_box, "TallBoxToHigh",
+ long_alpha_unitizer),
+ ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1,
+ tall_to_back_high_c2, back_middle2_box,
+ "TallBoxToMiddle2", long_alpha_unitizer),
+ ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
+ back_middle1_box, "TallBoxToMiddle1",
+ long_alpha_unitizer),
+ ThetaSplineSegment(tall_box_grab, tall_to_back_low_c1, tall_to_back_low_c2,
+ back_low_box, "TallBoxToLow", long_alpha_unitizer),
+ SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+ ready_above_box, "ReadyToNeutral"),
+ XYSegment(ready_above_box, tall_box_grab, "ReadyToTallBox", vmax=6.0),
+ XYSegment(ready_above_box, short_box_grab, "ReadyToShortBox", vmax=6.0),
+ XYSegment(tall_box_grab, short_box_grab, "TallToShortBox", vmax=6.0),
+ SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+ tall_box_grab, "TallToNeutral"),
+ SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+ short_box_grab, "ShortToNeutral"),
+ SplineSegment(neutral, up_c1, up_c2, up, "NeutralToUp"),
+ SplineSegment(neutral, front_high_box_c1, front_high_box_c2,
+ front_high_box, "NeutralToFrontHigh"),
+ SplineSegment(neutral, front_middle2_box_c1, front_middle2_box_c2,
+ front_middle2_box, "NeutralToFrontMiddle2"),
+ SplineSegment(neutral, front_middle1_box_c1, front_middle1_box_c2,
+ front_middle1_box, "NeutralToFrontMiddle1"),
+]
+
+unnamed_segments = [
+ SplineSegment(neutral, front_switch_auto_c1, front_switch_auto_c2,
+ front_switch_auto),
+ SplineSegment(tall_box_grab, ready_to_up_c1, ready_to_up_c2, up),
+ SplineSegment(short_box_grab, ready_to_up_c1, ready_to_up_c2, up),
+ SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2, up),
+ ThetaSplineSegment(duck, duck_c1, duck_c2, neutral),
+ SplineSegment(neutral, front_switch_c1, front_switch_c2, front_switch),
+ XYSegment(ready_above_box, front_low_box),
+ XYSegment(ready_above_box, front_switch),
+ XYSegment(ready_above_box, front_middle1_box),
+ XYSegment(ready_above_box, front_middle2_box),
+ XYSegment(ready_above_box, front_middle3_box),
+ SplineSegment(ready_above_box, ready_to_up_c1, ready_to_up_c2,
+ front_high_box),
+ AngleSegment(starting, vertical_starting),
+ AngleSegment(vertical_starting, neutral),
+ XYSegment(neutral, front_low_box),
+ XYSegment(up, front_high_box),
+ XYSegment(up, front_middle2_box),
+ XYSegment(front_middle3_box, up),
+ XYSegment(front_middle3_box, front_high_box),
+ XYSegment(front_middle3_box, front_middle2_box),
+ XYSegment(front_middle3_box, front_middle1_box),
+ XYSegment(up, front_middle1_box),
+ XYSegment(up, front_low_box),
+ XYSegment(front_high_box, front_middle2_box),
+ XYSegment(front_high_box, front_middle1_box),
+ XYSegment(front_high_box, front_low_box),
+ XYSegment(front_middle2_box, front_middle1_box),
+ XYSegment(front_middle2_box, front_low_box),
+ XYSegment(front_middle1_box, front_low_box),
+ XYSegment(front_switch, front_low_box),
+ XYSegment(front_switch, up),
+ XYSegment(front_switch, front_high_box),
+ AngleSegment(up, back_high_box),
+ AngleSegment(up, back_middle2_box),
+ AngleSegment(up, back_middle1_box),
+ AngleSegment(up, back_low_box),
+ XYSegment(back_high_box, back_middle2_box),
+ XYSegment(back_high_box, back_middle1_box),
+ XYSegment(back_high_box, back_low_box),
+ XYSegment(back_middle2_box, back_middle1_box),
+ XYSegment(back_middle2_box, back_low_box),
+ XYSegment(back_middle1_box, back_low_box),
+ AngleSegment(up, above_hang),
+ AngleSegment(above_hang, below_hang),
+ AngleSegment(up, below_hang),
+ AngleSegment(up, self_hang),
+ AngleSegment(up, partner_hang),
+] + front_paths + back_paths
+
+segments = []
+if isolate:
+ segments += named_segments[:isolate]
+else:
+ segments += named_segments + unnamed_segments
diff --git a/y2023/rockpi/BUILD b/y2023/rockpi/BUILD
new file mode 100644
index 0000000..91e8729
--- /dev/null
+++ b/y2023/rockpi/BUILD
@@ -0,0 +1,11 @@
+cc_binary(
+ name = "imu_main",
+ srcs = ["imu_main.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/imu_reader:imu",
+ ],
+)
diff --git a/y2023/rockpi/imu_main.cc b/y2023/rockpi/imu_main.cc
new file mode 100644
index 0000000..ac0c141
--- /dev/null
+++ b/y2023/rockpi/imu_main.cc
@@ -0,0 +1,24 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/realtime.h"
+#include "frc971/imu_reader/imu.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+ // TODO(austin): Set the ratio...
+ frc971::imu::Imu imu(&event_loop, 1.0);
+
+ event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+ event_loop.SetRuntimeRealtimePriority(55);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index 6ef0657..2ddb735 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -69,9 +69,11 @@
visibility = ["//y2023:__subpackages__"],
deps = [
"//aos:init",
+ "//aos:json_to_flatbuffer",
"//aos/events:shm_event_loop",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
+ "@com_google_absl//absl/strings",
],
)
@@ -86,6 +88,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2023:__subpackages__"],
deps = [
+ ":aprilrobotics_lib",
":calibration_data",
"//aos:init",
"//aos/events:simulated_event_loop",
@@ -97,3 +100,39 @@
"//third_party:opencv",
],
)
+
+cc_library(
+ name = "aprilrobotics_lib",
+ srcs = [
+ "aprilrobotics.cc",
+ "aprilrobotics.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ ":calibration_data",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:target_map_fbs",
+ "//frc971/vision:target_mapper",
+ "//frc971/vision:vision_fbs",
+ "//third_party:opencv",
+ "//third_party/apriltag",
+ ],
+)
+
+cc_binary(
+ name = "aprilrobotics",
+ srcs = [
+ "aprilrobotics_main.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ ":aprilrobotics_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
new file mode 100644
index 0000000..d20a247
--- /dev/null
+++ b/y2023/vision/aprilrobotics.cc
@@ -0,0 +1,175 @@
+#include "y2023/vision/aprilrobotics.h"
+
+DEFINE_bool(
+ debug, false,
+ "If true, dump a ton of debug and crash on the first valid detection.");
+
+DEFINE_int32(team_number, 971,
+ "Use the calibration for a node with this team number");
+namespace y2023 {
+namespace vision {
+
+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);
+ }),
+ target_map_sender_(
+ event_loop->MakeSender<frc971::vision::TargetMap>("/camera")) {
+ tag_family_ = tag16h5_create();
+ tag_detector_ = apriltag_detector_create();
+
+ apriltag_detector_add_family_bits(tag_detector_, tag_family_, 1);
+ tag_detector_->nthreads = 6;
+ tag_detector_->wp = workerpool_create(tag_detector_->nthreads);
+ tag_detector_->qtp.min_white_black_diff = 5;
+ tag_detector_->debug = FLAGS_debug;
+
+ std::string hostname = aos::network::GetHostname();
+
+ // Check team string is valid
+ std::optional<uint16_t> pi_number = aos::network::ParsePiNumber(hostname);
+ std::optional<uint16_t> team_number =
+ aos::network::team_number_internal::ParsePiTeamNumber(hostname);
+ CHECK(pi_number) << "Unable to parse pi number from '" << hostname << "'";
+ CHECK(team_number);
+
+ calibration_ = FindCameraCalibration(&calibration_data_.message(),
+ "pi" + std::to_string(*pi_number));
+ intrinsics_ = CameraIntrinsics(calibration_);
+ camera_distortion_coeffs_ = CameraDistCoeffs(calibration_);
+
+ image_callback_.set_format(frc971::vision::ImageCallback::Format::GRAYSCALE);
+}
+
+AprilRoboticsDetector::~AprilRoboticsDetector() {
+ apriltag_detector_destroy(tag_detector_);
+ free(tag_family_);
+}
+
+void AprilRoboticsDetector::SetWorkerpoolAffinities() {
+ for (int i = 0; i < tag_detector_->wp->nthreads; i++) {
+ cpu_set_t affinity;
+ CPU_ZERO(&affinity);
+ CPU_SET(i, &affinity);
+ pthread_setaffinity_np(tag_detector_->wp->threads[i], sizeof(affinity),
+ &affinity);
+ struct sched_param param;
+ param.sched_priority = 20;
+ int res = pthread_setschedparam(tag_detector_->wp->threads[i], SCHED_FIFO,
+ ¶m);
+ PCHECK(res == 0) << "Failed to set priority of threadpool threads";
+ }
+}
+
+void AprilRoboticsDetector::HandleImage(cv::Mat image_color_mat,
+ aos::monotonic_clock::time_point eof) {
+ std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> detections =
+ DetectTags(image_color_mat);
+
+ auto builder = target_map_sender_.MakeBuilder();
+ std::vector<flatbuffers::Offset<frc971::vision::TargetPoseFbs>> target_poses;
+ for (const auto &[detection, pose] : detections) {
+ target_poses.emplace_back(
+ BuildTargetPose(pose, detection.id, builder.fbb()));
+ }
+ const auto target_poses_offset = builder.fbb()->CreateVector(target_poses);
+ auto target_map_builder = builder.MakeBuilder<frc971::vision::TargetMap>();
+ target_map_builder.add_target_poses(target_poses_offset);
+ target_map_builder.add_monotonic_timestamp_ns(eof.time_since_epoch().count());
+ builder.CheckOk(builder.Send(target_map_builder.Finish()));
+}
+
+flatbuffers::Offset<frc971::vision::TargetPoseFbs>
+AprilRoboticsDetector::BuildTargetPose(
+ const apriltag_pose_t &pose,
+ frc971::vision::TargetMapper::TargetId target_id,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const auto T =
+ Eigen::Translation3d(pose.t->data[0], pose.t->data[1], pose.t->data[2]);
+ const auto position_offset =
+ frc971::vision::CreatePosition(*fbb, T.x(), T.y(), T.z());
+
+ const auto orientation = Eigen::Quaterniond(Eigen::Matrix3d(pose.R->data));
+ const auto orientation_offset = frc971::vision::CreateQuaternion(
+ *fbb, orientation.w(), orientation.x(), orientation.y(), orientation.z());
+
+ return frc971::vision::CreateTargetPoseFbs(*fbb, target_id, position_offset,
+ orientation_offset);
+}
+
+std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>>
+AprilRoboticsDetector::DetectTags(cv::Mat image) {
+ const aos::monotonic_clock::time_point start_time =
+ aos::monotonic_clock::now();
+
+ image_u8_t im = {
+ .width = image.cols,
+ .height = image.rows,
+ .stride = image.cols,
+ .buf = image.data,
+ };
+
+ ftrace_.FormatMessage("Starting detect\n");
+ zarray_t *detections = apriltag_detector_detect(tag_detector_, &im);
+ ftrace_.FormatMessage("Done detecting\n");
+
+ std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
+
+ for (int i = 0; i < zarray_size(detections); i++) {
+ apriltag_detection_t *det;
+ zarray_get(detections, i, &det);
+
+ if (det->decision_margin > 30) {
+ VLOG(1) << "Found tag number " << det->id << " hamming: " << det->hamming
+ << " margin: " << det->decision_margin;
+
+ const aos::monotonic_clock::time_point before_pose_estimation =
+ aos::monotonic_clock::now();
+ // First create an apriltag_detection_info_t struct using your known
+ // parameters.
+ apriltag_detection_info_t info;
+ info.det = det;
+ info.tagsize = 0.1524;
+ info.fx = intrinsics_.at<double>(0, 0);
+ info.fy = intrinsics_.at<double>(1, 1);
+ info.cx = intrinsics_.at<double>(0, 2);
+ info.cy = intrinsics_.at<double>(1, 2);
+
+ apriltag_pose_t pose;
+ double err = estimate_tag_pose(&info, &pose);
+
+ VLOG(1) << "err: " << err;
+
+ results.emplace_back(*det, pose);
+
+ const aos::monotonic_clock::time_point after_pose_estimation =
+ aos::monotonic_clock::now();
+
+ VLOG(1) << "Took "
+ << std::chrono::duration<double>(after_pose_estimation -
+ before_pose_estimation)
+ .count()
+ << " seconds for pose estimation";
+ }
+ }
+
+ apriltag_detections_destroy(detections);
+
+ const aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();
+
+ timeprofile_display(tag_detector_->tp);
+
+ VLOG(1) << "Took "
+ << std::chrono::duration<double>(end_time - start_time).count()
+ << " seconds to detect overall";
+
+ return results;
+}
+
+} // namespace vision
+} // namespace y2023
diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
new file mode 100644
index 0000000..a68b1d9
--- /dev/null
+++ b/y2023/vision/aprilrobotics.h
@@ -0,0 +1,101 @@
+
+#include <string>
+
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/network/team_number.h"
+#include "aos/realtime.h"
+#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/target_map_generated.h"
+#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/vision_generated.h"
+#include "opencv2/core/eigen.hpp"
+#include "opencv2/imgproc.hpp"
+#include "third_party/apriltag/apriltag.h"
+#include "third_party/apriltag/apriltag_pose.h"
+#include "third_party/apriltag/tag16h5.h"
+#include "y2023/vision/calibration_data.h"
+
+DECLARE_int32(team_number);
+
+namespace y2023 {
+namespace vision {
+
+class AprilRoboticsDetector {
+ public:
+ AprilRoboticsDetector(aos::EventLoop *event_loop,
+ std::string_view channel_name);
+
+ ~AprilRoboticsDetector();
+
+ void SetWorkerpoolAffinities();
+
+ std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> DetectTags(
+ cv::Mat image);
+
+ private:
+ void HandleImage(cv::Mat image, aos::monotonic_clock::time_point eof);
+
+ flatbuffers::Offset<frc971::vision::TargetPoseFbs> BuildTargetPose(
+ const apriltag_pose_t &pose,
+ frc971::vision::TargetMapper::TargetId target_id,
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ static const frc971::vision::calibration::CameraCalibration *
+ FindCameraCalibration(
+ const frc971::vision::calibration::CalibrationData *calibration_data,
+ std::string_view node_name) {
+ for (const frc971::vision::calibration::CameraCalibration *candidate :
+ *calibration_data->camera_calibrations()) {
+ if (candidate->node_name()->string_view() != node_name) {
+ continue;
+ }
+ if (candidate->team_number() != FLAGS_team_number) {
+ continue;
+ }
+ return candidate;
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+ << " on " << FLAGS_team_number;
+ }
+
+ static cv::Mat CameraIntrinsics(
+ const frc971::vision::calibration::CameraCalibration
+ *camera_calibration) {
+ cv::Mat result(3, 3, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->intrinsics()->data())));
+ result.convertTo(result, CV_64F);
+ CHECK_EQ(result.total(), camera_calibration->intrinsics()->size());
+
+ return result;
+ }
+
+ static cv::Mat CameraDistCoeffs(
+ const frc971::vision::calibration::CameraCalibration
+ *camera_calibration) {
+ const cv::Mat result(5, 1, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration->dist_coeffs()->data())));
+ CHECK_EQ(result.total(), camera_calibration->dist_coeffs()->size());
+ return result;
+ }
+
+ apriltag_family_t *tag_family_;
+ apriltag_detector_t *tag_detector_;
+
+ const aos::FlatbufferSpan<frc971::vision::calibration::CalibrationData>
+ calibration_data_;
+ const frc971::vision::calibration::CameraCalibration *calibration_;
+ cv::Mat intrinsics_;
+ cv::Mat camera_distortion_coeffs_;
+
+ aos::Ftrace ftrace_;
+
+ frc971::vision::ImageCallback image_callback_;
+ aos::Sender<frc971::vision::TargetMap> target_map_sender_;
+};
+
+} // namespace vision
+} // namespace y2023
diff --git a/y2023/vision/aprilrobotics_main.cc b/y2023/vision/aprilrobotics_main.cc
new file mode 100644
index 0000000..67b853f
--- /dev/null
+++ b/y2023/vision/aprilrobotics_main.cc
@@ -0,0 +1,34 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2023/vision/aprilrobotics.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+namespace y2023::vision {
+void AprilViewerMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ AprilRoboticsDetector detector(&event_loop, "/camera");
+
+ detector.SetWorkerpoolAffinities();
+
+ event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({5}));
+
+ struct sched_param param;
+ param.sched_priority = 21;
+ PCHECK(sched_setscheduler(0, SCHED_FIFO, ¶m) == 0);
+
+ event_loop.Run();
+}
+
+} // namespace y2023::vision
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2023::vision::AprilViewerMain();
+
+ return 0;
+}
diff --git a/y2023/vision/camera_reader.cc b/y2023/vision/camera_reader.cc
index a69caf3..b604516 100644
--- a/y2023/vision/camera_reader.cc
+++ b/y2023/vision/camera_reader.cc
@@ -16,6 +16,7 @@
DEFINE_double(red, 1.252, "Red gain");
DEFINE_double(green, 1, "Green gain");
DEFINE_double(blue, 1.96, "Blue gain");
+DEFINE_double(exposure, 150, "Camera exposure");
namespace y2023 {
namespace vision {
@@ -62,8 +63,8 @@
media_device->FindEntity("rkisp1_resizer_selfpath");
rkisp1_resizer_selfpath->pads(0)->SetSubdevFormat(width, height,
MEDIA_BUS_FMT_YUYV8_2X8);
- rkisp1_resizer_selfpath->pads(1)->SetSubdevFormat(width, height,
- MEDIA_BUS_FMT_YUYV8_2X8);
+ rkisp1_resizer_selfpath->pads(1)->SetSubdevFormat(
+ width * 2 / 3, height * 2 / 3, MEDIA_BUS_FMT_YUYV8_2X8);
rkisp1_resizer_selfpath->pads(0)->SetSubdevCrop(width, height);
Entity *rkisp1_resizer_mainpath =
@@ -79,7 +80,7 @@
rkisp1_mainpath->SetFormat(width / 2, height / 2, V4L2_PIX_FMT_YUV422P);
Entity *rkisp1_selfpath = media_device->FindEntity("rkisp1_selfpath");
- rkisp1_selfpath->SetFormat(width, height, V4L2_PIX_FMT_YUYV);
+ rkisp1_selfpath->SetFormat(width * 2 / 3, height * 2 / 3, V4L2_PIX_FMT_YUYV);
media_device->Enable(
media_device->FindLink(camera_device_string, 0, "rkisp1_csi", 0));
@@ -104,7 +105,7 @@
if (FLAGS_lowlight_camera) {
v4l2_reader.SetGainExt(100);
v4l2_reader.SetVerticalBlanking(1000);
- v4l2_reader.SetExposure(150);
+ v4l2_reader.SetExposure(FLAGS_exposure);
} else {
v4l2_reader.SetGainExt(1000);
v4l2_reader.SetExposure(1000);
diff --git a/y2023/vision/target_map.json b/y2023/vision/target_map.json
index 5f256fa..3f6eb54 100644
--- a/y2023/vision/target_map.json
+++ b/y2023/vision/target_map.json
@@ -2,75 +2,123 @@
"target_poses": [
{
"id": 1,
- "x": 7.244,
- "y": -2.938,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.244,
+ "y": -2.938,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 2,
- "x": 7.244,
- "y": -1.262,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.244,
+ "y": -1.262,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 3,
- "x": 7.244,
- "y": 0.414,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.244,
+ "y": 0.414,
+ "z": 0.463
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 4,
- "x": 7.909,
- "y": 2.740,
- "z": 0.695,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 3.141592653589793
+ "position": {
+ "x": 7.909,
+ "y": 2.740,
+ "z": 0.695
+ },
+ /* yaw of pi */
+ "orientation": {
+ "w": 0.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 1.0
+ }
},
{
"id": 5,
- "x": -7.908,
- "y": 2.740,
- "z": 0.695,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.908,
+ "y": 2.740,
+ "z": 0.695
+ },
+ "orientation": {
+ /* yaw of 0 */
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
},
{
"id": 6,
- "x": -7.243,
- "y": 0.414,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.243,
+ "y": 0.414,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
},
{
"id": 7,
- "x": -7.243,
- "y": -1.262,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.243,
+ "y": -1.262,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
},
{
"id": 8,
- "x": -7.243,
- "y": -2.938,
- "z": 0.463,
- "roll": 0.0,
- "pitch": 0.0,
- "yaw": 0.0
+ "position": {
+ "x": -7.243,
+ "y": -2.938,
+ "z": 0.463
+ },
+ /* yaw of 0 */
+ "orientation": {
+ "w": 1.0,
+ "x": 0.0,
+ "y": 0.0,
+ "z": 0.0
+ }
}
]
}
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index d4c8c50..6d32dbd 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -12,14 +12,12 @@
#include "opencv2/highgui.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc.hpp"
+#include "y2023/vision/aprilrobotics.h"
#include "y2023/vision/calibration_data.h"
DEFINE_string(json_path, "target_map.json",
"Specify path for json with initial pose guesses.");
-DEFINE_int32(team_number, 7971,
- "Use the calibration for a node with this team number");
-
-DECLARE_string(image_channel);
+DECLARE_int32(team_number);
namespace y2023 {
namespace vision {
@@ -27,6 +25,7 @@
using frc971::vision::DataAdapter;
using frc971::vision::ImageCallback;
using frc971::vision::PoseUtils;
+using frc971::vision::TargetMap;
using frc971::vision::TargetMapper;
namespace calibration = frc971::vision::calibration;
@@ -40,21 +39,17 @@
// Add detected apriltag poses relative to the robot to
// timestamped_target_detections
-void HandleAprilTag(aos::distributed_clock::time_point pi_distributed_time,
- std::vector<cv::Vec4i> april_ids,
- std::vector<Eigen::Vector3d> rvecs_eigen,
- std::vector<Eigen::Vector3d> tvecs_eigen,
+void HandleAprilTag(const TargetMap &map,
+ aos::distributed_clock::time_point pi_distributed_time,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
Eigen::Affine3d extrinsics) {
- for (size_t tag = 0; tag < april_ids.size(); tag++) {
- Eigen::Translation3d T_camera_target = Eigen::Translation3d(
- tvecs_eigen[tag][0], tvecs_eigen[tag][1], tvecs_eigen[tag][2]);
- Eigen::AngleAxisd r_angle = Eigen::AngleAxisd(
- rvecs_eigen[tag].norm(), rvecs_eigen[tag] / rvecs_eigen[tag].norm());
- CHECK(rvecs_eigen[tag].norm() != 0) << "rvecs norm = 0; divide by 0";
+ for (const auto *target_pose_fbs : *map.target_poses()) {
+ const TargetMapper::TargetPose target_pose =
+ PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
- Eigen::Affine3d H_camcv_target = T_camera_target * r_angle;
+ Eigen::Affine3d H_camcv_target =
+ Eigen::Translation3d(target_pose.pose.p) * target_pose.pose.q;
// With X, Y, Z being robot axes and x, y, z being camera axes,
// x = -Y, y = -Z, z = X
static const Eigen::Affine3d H_camcv_camrob =
@@ -69,12 +64,15 @@
PoseUtils::Affine3dToPose3d(H_camrob_target);
double distance_from_camera = target_pose_camera.p.norm();
+ CHECK(map.has_monotonic_timestamp_ns())
+ << "Need detection timestamps for mapping";
+
timestamped_target_detections->emplace_back(
DataAdapter::TimestampedDetection{
.time = pi_distributed_time,
.H_robot_target = H_robot_target,
.distance_from_camera = distance_from_camera,
- .id = april_ids[tag][0]});
+ .id = static_cast<TargetMapper::TargetId>(target_pose.id)});
}
}
@@ -115,49 +113,33 @@
// Get images from pi and pass apriltag positions to HandleAprilTag()
void HandlePiCaptures(
- int pi_number, aos::EventLoop *pi_event_loop,
- aos::logger::LogReader *reader,
+ aos::EventLoop *pi_event_loop, aos::logger::LogReader *reader,
std::vector<DataAdapter::TimestampedDetection>
*timestamped_target_detections,
- std::vector<std::unique_ptr<CharucoExtractor>> *charuco_extractors,
- std::vector<std::unique_ptr<ImageCallback>> *image_callbacks) {
+ std::vector<std::unique_ptr<AprilRoboticsDetector>> *detectors) {
const aos::FlatbufferSpan<calibration::CalibrationData> calibration_data(
CalibrationData());
- const calibration::CameraCalibration *calibration = FindCameraCalibration(
- &calibration_data.message(), "pi" + std::to_string(pi_number));
+
+ const auto node_name = pi_event_loop->node()->name()->string_view();
+ const calibration::CameraCalibration *calibration =
+ FindCameraCalibration(&calibration_data.message(), node_name);
const auto extrinsics = CameraExtrinsics(calibration);
// TODO(milind): change to /camera once we log at full frequency
static constexpr std::string_view kImageChannel = "/camera/decimated";
- charuco_extractors->emplace_back(std::make_unique<CharucoExtractor>(
- pi_event_loop,
- "pi-" + std::to_string(FLAGS_team_number) + "-" +
- std::to_string(pi_number),
- frc971::vision::TargetType::kAprilTag, kImageChannel,
- [=](cv::Mat /*rgb_image*/, aos::monotonic_clock::time_point eof,
- std::vector<cv::Vec4i> april_ids,
- std::vector<std::vector<cv::Point2f>> /*april_corners*/, bool valid,
- std::vector<Eigen::Vector3d> rvecs_eigen,
- std::vector<Eigen::Vector3d> tvecs_eigen) {
- aos::distributed_clock::time_point pi_distributed_time =
- reader->event_loop_factory()
- ->GetNodeEventLoopFactory(pi_event_loop->node())
- ->ToDistributedClock(eof);
+ detectors->emplace_back(
+ std::make_unique<AprilRoboticsDetector>(pi_event_loop, kImageChannel));
- if (valid) {
- HandleAprilTag(pi_distributed_time, april_ids, rvecs_eigen,
- tvecs_eigen, timestamped_target_detections,
- extrinsics);
- }
- }));
+ pi_event_loop->MakeWatcher("/camera", [=](const TargetMap &map) {
+ aos::distributed_clock::time_point pi_distributed_time =
+ reader->event_loop_factory()
+ ->GetNodeEventLoopFactory(pi_event_loop->node())
+ ->ToDistributedClock(aos::monotonic_clock::time_point(
+ aos::monotonic_clock::duration(map.monotonic_timestamp_ns())));
- image_callbacks->emplace_back(std::make_unique<ImageCallback>(
- pi_event_loop, kImageChannel,
- [&, charuco_extractor =
- charuco_extractors->at(charuco_extractors->size() - 1).get()](
- cv::Mat rgb_image, const aos::monotonic_clock::time_point eof) {
- charuco_extractor->HandleImage(rgb_image, eof);
- }));
+ HandleAprilTag(map, pi_distributed_time, timestamped_target_detections,
+ extrinsics);
+ });
}
void MappingMain(int argc, char *argv[]) {
@@ -170,40 +152,35 @@
aos::logger::LogReader reader(aos::logger::SortParts(unsorted_logfiles));
reader.Register();
- std::vector<std::unique_ptr<CharucoExtractor>> charuco_extractors;
- std::vector<std::unique_ptr<ImageCallback>> image_callbacks;
+ std::vector<std::unique_ptr<AprilRoboticsDetector>> detectors;
const aos::Node *pi1 =
aos::configuration::GetNode(reader.configuration(), "pi1");
std::unique_ptr<aos::EventLoop> pi1_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi1", pi1);
- HandlePiCaptures(1, pi1_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ HandlePiCaptures(pi1_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
const aos::Node *pi2 =
aos::configuration::GetNode(reader.configuration(), "pi2");
std::unique_ptr<aos::EventLoop> pi2_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi2", pi2);
- HandlePiCaptures(2, pi2_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ HandlePiCaptures(pi2_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
const aos::Node *pi3 =
aos::configuration::GetNode(reader.configuration(), "pi3");
std::unique_ptr<aos::EventLoop> pi3_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi3", pi3);
- HandlePiCaptures(3, pi3_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ HandlePiCaptures(pi3_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
const aos::Node *pi4 =
aos::configuration::GetNode(reader.configuration(), "pi4");
std::unique_ptr<aos::EventLoop> pi4_event_loop =
reader.event_loop_factory()->MakeEventLoop("pi4", pi4);
- HandlePiCaptures(4, pi4_event_loop.get(), &reader,
- ×tamped_target_detections, &charuco_extractors,
- &image_callbacks);
+ HandlePiCaptures(pi4_event_loop.get(), &reader,
+ ×tamped_target_detections, &detectors);
reader.event_loop_factory()->Run();
@@ -211,15 +188,11 @@
DataAdapter::MatchTargetDetections(timestamped_target_detections);
frc971::vision::TargetMapper mapper(FLAGS_json_path, target_constraints);
- mapper.Solve("rapid_react");
+ mapper.Solve("charged_up");
- // Pointers need to be deleted to destruct all fetchers
- for (auto &charuco_extractor_ptr : charuco_extractors) {
- charuco_extractor_ptr.reset();
- }
-
- for (auto &image_callback_ptr : image_callbacks) {
- image_callback_ptr.reset();
+ // Clean up all the pointers
+ for (auto &detector_ptr : detectors) {
+ detector_ptr.reset();
}
}
diff --git a/y2023/vision/viewer.cc b/y2023/vision/viewer.cc
index 990ad7a..08def5b 100644
--- a/y2023/vision/viewer.cc
+++ b/y2023/vision/viewer.cc
@@ -1,8 +1,10 @@
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
+#include "absl/strings/match.h"
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
#include "aos/time/time.h"
#include "frc971/vision/vision_generated.h"
@@ -36,7 +38,12 @@
cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
if (!FLAGS_capture.empty()) {
- cv::imwrite(FLAGS_capture, bgr_image);
+ if (absl::EndsWith(FLAGS_capture, ".bfbs")) {
+ aos::WriteFlatbufferToFile(FLAGS_capture, image_fetcher.CopyFlatBuffer());
+ } else {
+ cv::imwrite(FLAGS_capture, bgr_image);
+ }
+
return false;
}
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 fd3e97f..fac37b2 100644
--- a/y2023/y2023_pi_template.json
+++ b/y2023/y2023_pi_template.json
@@ -164,12 +164,59 @@
"type": "frc971.vision.CameraImage",
"source_node": "pi{{ NUM }}",
"frequency": 40,
- "max_size": 4200000,
+ "max_size": 1843456,
"num_readers": 4,
"read_method": "PIN",
"num_senders": 18
},
{
+ "name": "/pi{{ NUM }}/camera",
+ "type": "frc971.vision.TargetMap",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 40,
+ "num_senders": 2,
+ "max_size": 40000,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu",
+ "logger"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 4,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "pi{{ NUM }}"
+ ],
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 4,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "pi{{ NUM }}"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/pi{{ NUM }}/aos/remote_timestamps/imu/pi{{ NUM }}/camera/frc971-vision-TargetMap",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 80,
+ "source_node": "pi{{ NUM }}",
+ "max_size": 208
+ },
+ {
+ "name": "/pi{{ NUM }}/aos/remote_timestamps/logger/pi{{ NUM }}/camera/frc971-vision-TargetMap",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 80,
+ "source_node": "pi{{ NUM }}",
+ "max_size": 208
+ },
+ {
"name": "/logger/aos",
"type": "aos.starter.StarterRpc",
"source_node": "logger",