Defer requeuing buffers in v4l2 reader
Enqueuing buffers takes a long time, so we want to move it out of the
way of images being sent.
Requeue the buffers in a worker thread that's running at a lower
priority.
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
Change-Id: I3887bca9099916b1864ff88893b940d6a31f7edf
diff --git a/aos/util/BUILD b/aos/util/BUILD
index 48dc054..929b376 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -466,6 +466,28 @@
)
cc_library(
+ name = "threaded_consumer",
+ hdrs = [
+ "threaded_consumer.h",
+ ],
+ deps = [
+ "//aos:condition",
+ "//aos:realtime",
+ "//aos/containers:ring_buffer",
+ "//aos/mutex",
+ ],
+)
+
+cc_test(
+ name = "threaded_consumer_test",
+ srcs = ["threaded_consumer_test.cc"],
+ deps = [
+ ":threaded_consumer",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
name = "foxglove_websocket_lib",
srcs = ["foxglove_websocket_lib.cc"],
hdrs = ["foxglove_websocket_lib.h"],
diff --git a/aos/util/threaded_consumer.h b/aos/util/threaded_consumer.h
new file mode 100644
index 0000000..95ec79a
--- /dev/null
+++ b/aos/util/threaded_consumer.h
@@ -0,0 +1,102 @@
+#ifndef AOS_UTIL_THREADED_CONSUMER_H_
+#define AOS_UTIL_THREADED_CONSUMER_H_
+
+#include <functional>
+#include <optional>
+#include <thread>
+
+#include "aos/condition.h"
+#include "aos/containers/ring_buffer.h"
+#include "aos/mutex/mutex.h"
+#include "aos/realtime.h"
+
+namespace aos {
+namespace util {
+
+// This class implements a threadpool of a single worker that accepts work
+// from the main thread through a queue and executes it at a different realtime
+// priority.
+//
+// There is no mechanism to get data back to the main thread, the worker only
+// acts as a consumer. When this class is destroyed, it join()s the worker and
+// finishes all outstanding tasks.
+template <typename T, int QueueSize>
+class ThreadedConsumer {
+ public:
+ // Constructs a new ThreadedConsumer with the given consumer function to be
+ // run at the given realtime priority. If worker_priority is zero, the thread
+ // will stay at non realtime priority.
+ ThreadedConsumer(std::function<void(T)> consumer_function,
+ int worker_priority)
+ : consumer_function_(consumer_function),
+ worker_priority_(worker_priority),
+ more_tasks_(&mutex_),
+ worker_thread_([this]() { WorkerFunction(); }) {}
+
+ ~ThreadedConsumer() {
+ {
+ aos::MutexLocker locker(&mutex_);
+ quit_ = true;
+ more_tasks_.Broadcast();
+ }
+ worker_thread_.join();
+ }
+
+ // Submits another task to be processed by the worker.
+ // Returns true if successfully pushed onto the queue, and false if the queue
+ // is full.
+ bool Push(T task) {
+ aos::MutexLocker locker(&mutex_);
+
+ if (task_queue_.full()) {
+ return false;
+ }
+
+ task_queue_.Push(task);
+ more_tasks_.Broadcast();
+
+ return true;
+ }
+
+ private:
+ void WorkerFunction() {
+ if (worker_priority_ > 0) {
+ aos::SetCurrentThreadRealtimePriority(worker_priority_);
+ }
+
+ while (true) {
+ std::optional<T> task;
+
+ {
+ aos::MutexLocker locker(&mutex_);
+ while (task_queue_.empty() && !quit_) {
+ CHECK(!more_tasks_.Wait());
+ }
+
+ if (task_queue_.empty() && quit_) break;
+
+ // Pop
+ task = std::move(task_queue_[0]);
+ task_queue_.Shift();
+ }
+
+ consumer_function_(*task);
+ task.reset();
+ }
+
+ aos::UnsetCurrentThreadRealtimePriority();
+ }
+
+ std::function<void(T)> consumer_function_;
+ aos::RingBuffer<T, QueueSize> task_queue_;
+ aos::Mutex mutex_;
+ bool quit_ = false;
+ int worker_priority_;
+ aos::Condition more_tasks_;
+ std::thread worker_thread_;
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_UTIL_THREADWORKER_H_
diff --git a/aos/util/threaded_consumer_test.cc b/aos/util/threaded_consumer_test.cc
new file mode 100644
index 0000000..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