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/frc971/vision/BUILD b/frc971/vision/BUILD
index c52afae..4dbd66c 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -76,6 +76,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/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