Moving v4l2_reader into frc971/vision. Fixing dependencies
Change-Id: I25e821b7fb77a6c183dfeb697b81c771cd5d2339
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
new file mode 100644
index 0000000..1e3ed58
--- /dev/null
+++ b/frc971/vision/BUILD
@@ -0,0 +1,35 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
+
+flatbuffer_cc_library(
+ name = "vision_fbs",
+ srcs = ["vision.fbs"],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_ts_library(
+ name = "vision_ts_fbs",
+ srcs = ["vision.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "v4l2_reader",
+ srcs = [
+ "v4l2_reader.cc",
+ ],
+ hdrs = [
+ "v4l2_reader.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":vision_fbs",
+ "//aos/events:event_loop",
+ "//aos/scoped:scoped_fd",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/base",
+ ],
+)
diff --git a/frc971/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
new file mode 100644
index 0000000..793d8cd
--- /dev/null
+++ b/frc971/vision/v4l2_reader.cc
@@ -0,0 +1,201 @@
+#include "frc971/vision/v4l2_reader.h"
+
+#include <fcntl.h>
+#include <linux/videodev2.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+DEFINE_bool(ignore_timestamps, false,
+ "Don't require timestamps on images. Used to allow webcams");
+
+namespace frc971 {
+namespace vision {
+
+V4L2Reader::V4L2Reader(aos::EventLoop *event_loop,
+ const std::string &device_name)
+ : fd_(open(device_name.c_str(), O_RDWR | O_NONBLOCK)) {
+ PCHECK(fd_.get() != -1);
+
+ // First, clean up after anybody else who left the device streaming.
+ StreamOff();
+
+ {
+ struct v4l2_format format;
+ memset(&format, 0, sizeof(format));
+ format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ format.fmt.pix.width = cols_;
+ format.fmt.pix.height = rows_;
+ format.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+ // This means we want to capture from a progressive (non-interlaced) source.
+ format.fmt.pix.field = V4L2_FIELD_NONE;
+ PCHECK(Ioctl(VIDIOC_S_FMT, &format) == 0);
+ CHECK_EQ(static_cast<int>(format.fmt.pix.width), cols_);
+ CHECK_EQ(static_cast<int>(format.fmt.pix.height), rows_);
+ CHECK_EQ(static_cast<int>(format.fmt.pix.bytesperline),
+ cols_ * 2 /* bytes per pixel */);
+ CHECK_EQ(format.fmt.pix.sizeimage, ImageSize());
+ }
+
+ {
+ struct v4l2_requestbuffers request;
+ memset(&request, 0, sizeof(request));
+ request.count = buffers_.size();
+ request.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ request.memory = V4L2_MEMORY_USERPTR;
+ PCHECK(Ioctl(VIDIOC_REQBUFS, &request) == 0);
+ CHECK_EQ(request.count, buffers_.size())
+ << ": Kernel refused to give us the number of buffers we asked for";
+ }
+
+ for (size_t i = 0; i < buffers_.size(); ++i) {
+ buffers_[i].sender = event_loop->MakeSender<CameraImage>("/camera");
+ EnqueueBuffer(i);
+ }
+
+ {
+ int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ PCHECK(Ioctl(VIDIOC_STREAMON, &type) == 0);
+ }
+}
+
+bool V4L2Reader::ReadLatestImage() {
+ // First, enqueue any old buffer we already have. This is the one which
+ // may have been sent.
+ if (saved_buffer_) {
+ EnqueueBuffer(saved_buffer_.index);
+ saved_buffer_.Clear();
+ }
+ while (true) {
+ const BufferInfo previous_buffer = saved_buffer_;
+ saved_buffer_ = DequeueBuffer();
+ if (saved_buffer_) {
+ // We got a new buffer. Return the previous one (if relevant) and keep
+ // going.
+ if (previous_buffer) {
+ EnqueueBuffer(previous_buffer.index);
+ }
+ continue;
+ }
+ if (!previous_buffer) {
+ // There were no images to read. Return an indication of that.
+ return false;
+ }
+ // We didn't get a new one, but we already got one in a previous
+ // iteration, which means we found an image so return it.
+ saved_buffer_ = previous_buffer;
+ buffers_[saved_buffer_.index].PrepareMessage(rows_, cols_, ImageSize(),
+ saved_buffer_.monotonic_eof);
+ return true;
+ }
+}
+
+void V4L2Reader::SendLatestImage() { buffers_[saved_buffer_.index].Send(); }
+
+void V4L2Reader::SetExposure(size_t duration) {
+ v4l2_control manual_control;
+ manual_control.id = V4L2_CID_EXPOSURE_AUTO;
+ manual_control.value = V4L2_EXPOSURE_MANUAL;
+ PCHECK(Ioctl(VIDIOC_S_CTRL, &manual_control) == 0);
+
+ v4l2_control exposure_control;
+ exposure_control.id = V4L2_CID_EXPOSURE_ABSOLUTE;
+ exposure_control.value = static_cast<int>(duration); // 100 micro s units
+ PCHECK(Ioctl(VIDIOC_S_CTRL, &exposure_control) == 0);
+}
+
+void V4L2Reader::UseAutoExposure() {
+ v4l2_control control;
+ control.id = V4L2_CID_EXPOSURE_AUTO;
+ control.value = V4L2_EXPOSURE_AUTO;
+ PCHECK(Ioctl(VIDIOC_S_CTRL, &control) == 0);
+}
+
+void V4L2Reader::Buffer::InitializeMessage(size_t max_image_size) {
+ message_offset = flatbuffers::Offset<CameraImage>();
+ builder = aos::Sender<CameraImage>::Builder();
+ builder = sender.MakeBuilder();
+ // The kernel has an undocumented requirement that the buffer is aligned
+ // to 64 bytes. If you give it a nonaligned pointer, it will return EINVAL
+ // and only print something in dmesg with the relevant dynamic debug
+ // prints turned on.
+ builder.fbb()->StartIndeterminateVector(max_image_size, 1, 64, &data_pointer);
+ CHECK_EQ(reinterpret_cast<uintptr_t>(data_pointer) % 64, 0u)
+ << ": Flatbuffers failed to align things as requested";
+}
+
+void V4L2Reader::Buffer::PrepareMessage(
+ int rows, int cols, size_t image_size,
+ aos::monotonic_clock::time_point monotonic_eof) {
+ CHECK(data_pointer != nullptr);
+ data_pointer = nullptr;
+
+ const auto data_offset = builder.fbb()->EndIndeterminateVector(image_size, 1);
+ auto image_builder = builder.MakeBuilder<CameraImage>();
+ image_builder.add_data(data_offset);
+ image_builder.add_rows(rows);
+ image_builder.add_cols(cols);
+ image_builder.add_monotonic_timestamp_ns(
+ std::chrono::nanoseconds(monotonic_eof.time_since_epoch()).count());
+ message_offset = image_builder.Finish();
+}
+
+int V4L2Reader::Ioctl(unsigned long number, void *arg) {
+ return ioctl(fd_.get(), number, arg);
+}
+
+V4L2Reader::BufferInfo V4L2Reader::DequeueBuffer() {
+ struct v4l2_buffer buffer;
+ memset(&buffer, 0, sizeof(buffer));
+ buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buffer.memory = V4L2_MEMORY_USERPTR;
+ const int result = Ioctl(VIDIOC_DQBUF, &buffer);
+ if (result == -1 && errno == EAGAIN) {
+ return BufferInfo();
+ }
+ PCHECK(result == 0) << ": VIDIOC_DQBUF failed";
+ CHECK_LT(buffer.index, buffers_.size());
+ CHECK_EQ(reinterpret_cast<uintptr_t>(buffers_[buffer.index].data_pointer),
+ buffer.m.userptr);
+ CHECK_EQ(ImageSize(), buffer.length);
+ CHECK(buffer.flags & V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC);
+ if (!FLAGS_ignore_timestamps) {
+ // Require that we have good timestamp on images
+ CHECK_EQ(buffer.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK,
+ static_cast<uint32_t>(V4L2_BUF_FLAG_TSTAMP_SRC_EOF));
+ }
+ return {static_cast<int>(buffer.index),
+ aos::time::from_timeval(buffer.timestamp)};
+}
+
+void V4L2Reader::EnqueueBuffer(int buffer_number) {
+ CHECK_GE(buffer_number, 0);
+ CHECK_LT(buffer_number, static_cast<int>(buffers_.size()));
+ buffers_[buffer_number].InitializeMessage(ImageSize());
+ struct v4l2_buffer buffer;
+ memset(&buffer, 0, sizeof(buffer));
+ buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buffer.memory = V4L2_MEMORY_USERPTR;
+ buffer.index = buffer_number;
+ buffer.m.userptr =
+ reinterpret_cast<uintptr_t>(buffers_[buffer_number].data_pointer);
+ buffer.length = ImageSize();
+ PCHECK(Ioctl(VIDIOC_QBUF, &buffer) == 0);
+}
+
+void V4L2Reader::StreamOff() {
+ int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ const int result = Ioctl(VIDIOC_STREAMOFF, &type);
+ if (result == 0) {
+ return;
+ }
+ // Some devices (like Alex's webcam) return this if streaming isn't
+ // currently on, unlike what the documentations says should happen.
+ if (errno == EBUSY) {
+ return;
+ }
+ PLOG(FATAL) << "VIDIOC_STREAMOFF failed";
+}
+
+} // namespace vision
+} // namespace frc971
diff --git a/frc971/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
new file mode 100644
index 0000000..6f90cec
--- /dev/null
+++ b/frc971/vision/v4l2_reader.h
@@ -0,0 +1,122 @@
+#ifndef FRC971_VISION_V4L2_READER_H_
+#define FRC971_VISION_V4L2_READER_H_
+
+#include <array>
+#include <string>
+
+#include "absl/types/span.h"
+#include "glog/logging.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/scoped/scoped_fd.h"
+#include "frc971/vision/vision_generated.h"
+
+namespace frc971 {
+namespace vision {
+
+// Reads images from a V4L2 capture device (aka camera).
+class V4L2Reader {
+ public:
+ // device_name is the name of the device file (like "/dev/video0").
+ V4L2Reader(aos::EventLoop *event_loop, const std::string &device_name);
+
+ V4L2Reader(const V4L2Reader &) = delete;
+ V4L2Reader &operator=(const V4L2Reader &) = delete;
+
+ // Reads the latest image.
+ //
+ // Returns false if no image was available since the last image was read.
+ // Call LatestImage() to get a reference to the data, which will be valid
+ // until this method is called again.
+ bool ReadLatestImage();
+
+ // Sends the latest image.
+ //
+ // ReadLatestImage() must have returned a non-empty span the last time it was
+ // called. After calling this, the data which was returned from
+ // ReadLatestImage() will no longer be valid.
+ void SendLatestImage();
+
+ const CameraImage &LatestImage() {
+ Buffer *const buffer = &buffers_[saved_buffer_.index];
+ return *flatbuffers::GetTemporaryPointer(*buffer->builder.fbb(),
+ buffer->message_offset);
+ }
+
+ // Sets the exposure duration of the camera. duration is the number of 100
+ // microsecond units.
+ void SetExposure(size_t duration);
+
+ // Switches from manual to auto exposure.
+ void UseAutoExposure();
+
+ private:
+ static constexpr int kNumberBuffers = 16;
+
+ struct Buffer {
+ void InitializeMessage(size_t max_image_size);
+
+ void PrepareMessage(int rows, int cols, size_t image_size,
+ aos::monotonic_clock::time_point monotonic_eof);
+
+ void Send() {
+ (void)builder.Send(message_offset);
+ message_offset = flatbuffers::Offset<CameraImage>();
+ }
+
+ absl::Span<const char> DataSpan(size_t image_size) {
+ return absl::Span<const char>(
+ reinterpret_cast<char *>(CHECK_NOTNULL(data_pointer)), image_size);
+ }
+
+ aos::Sender<CameraImage> sender;
+ aos::Sender<CameraImage>::Builder builder;
+ flatbuffers::Offset<CameraImage> message_offset;
+
+ uint8_t *data_pointer = nullptr;
+ };
+
+ struct BufferInfo {
+ int index = -1;
+ aos::monotonic_clock::time_point monotonic_eof =
+ aos::monotonic_clock::min_time;
+
+ explicit operator bool() const { return index != -1; }
+
+ void Clear() {
+ index = -1;
+ monotonic_eof = aos::monotonic_clock::min_time;
+ }
+ };
+
+ // TODO(Brian): This concept won't exist once we start using variable-size
+ // H.264 frames.
+ size_t ImageSize() const { return rows_ * cols_ * 2 /* bytes per pixel */; }
+
+ // Attempts to dequeue a buffer (nonblocking). Returns the index of the new
+ // buffer, or BufferInfo() if there wasn't a frame to dequeue.
+ BufferInfo DequeueBuffer();
+
+ void EnqueueBuffer(int buffer);
+
+ int Ioctl(unsigned long number, void *arg);
+
+ void StreamOff();
+
+ // The mmaped V4L2 buffers.
+ std::array<Buffer, kNumberBuffers> buffers_;
+
+ // If this is non-negative, it's the buffer number we're currently holding
+ // onto.
+ BufferInfo saved_buffer_;
+
+ const int rows_ = 480;
+ const int cols_ = 640;
+
+ aos::ScopedFD fd_;
+};
+
+} // namespace vision
+} // namespace frc971
+
+#endif // FRC971_VISION_V4L2_READER_H_
diff --git a/frc971/vision/vision.fbs b/frc971/vision/vision.fbs
new file mode 100644
index 0000000..e89a181
--- /dev/null
+++ b/frc971/vision/vision.fbs
@@ -0,0 +1,22 @@
+namespace frc971.vision;
+
+// Contains the image data from one frame of a camera.
+//
+// The following image options are hard-coded. If you add images in a different
+// format, make fields for them which default to these values and remove this
+// comment:
+// * Format: YUYV (V4L2_PIX_FMT_YUYV, which puts 2 pixels in every 4 bytes,
+// with the order Y0,U,Y1,V)
+// * Order: row major (index 0 is upper left, index 1 is to its right)
+table CameraImage {
+ // The number of rows in the image.
+ rows:int32 (id: 0);
+ // The number of columns in the image.
+ cols:int32 (id: 1);
+ // The image data.
+ data:[ubyte] (id: 2);
+ // Timestamp when the frame was captured. This is the end-of-frame timestamp.
+ monotonic_timestamp_ns:int64 (id: 3);
+}
+
+root_type CameraImage;