Add C++ PNG/JPEG converter for foxglove CompressedImage
This gives us a converter that we can slot into C++ code to convert our
CameraImage type into foxglove CompressedImage types, for more efficient
display.
Change-Id: I6e831c341ca90ca26b38af504c1bb86d482cf561
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 094021d..c52afae 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -251,3 +251,16 @@
"@com_google_absl//absl/strings",
],
)
+
+cc_library(
+ name = "foxglove_image_converter",
+ srcs = ["foxglove_image_converter.cc"],
+ hdrs = ["foxglove_image_converter.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":vision_fbs",
+ "//aos/events:event_loop",
+ "//third_party:opencv",
+ "@com_github_foxglove_schemas//:schemas",
+ ],
+)
diff --git a/frc971/vision/foxglove_image_converter.cc b/frc971/vision/foxglove_image_converter.cc
new file mode 100644
index 0000000..0c5c736
--- /dev/null
+++ b/frc971/vision/foxglove_image_converter.cc
@@ -0,0 +1,59 @@
+#include "frc971/vision/foxglove_image_converter.h"
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+
+namespace frc971::vision {
+namespace {
+std::string_view ExtensionForCompression(ImageCompression compression) {
+ switch (compression) {
+ case ImageCompression::kJpeg:
+ return "jpeg";
+ case ImageCompression::kPng:
+ return "png";
+ }
+}
+} // namespace
+flatbuffers::Offset<foxglove::CompressedImage> CompressImage(
+ const CameraImage *raw_image, flatbuffers::FlatBufferBuilder *fbb,
+ ImageCompression compression) {
+ std::string_view format = ExtensionForCompression(compression);
+ // imencode doesn't let us pass in anything other than an std::vector, and
+ // performance isn't yet a big enough issue to try to avoid the copy.
+ std::vector<uint8_t> buffer;
+ CHECK(raw_image->has_data());
+ cv::Mat image_color_mat(cv::Size(raw_image->cols(), raw_image->rows()),
+ CV_8UC2, (void *)raw_image->data()->data());
+ cv::Mat bgr_image(cv::Size(raw_image->cols(), raw_image->rows()), CV_8UC3);
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
+ CHECK(cv::imencode(absl::StrCat(".", format), bgr_image, buffer));
+ const flatbuffers::Offset<flatbuffers::Vector<uint8_t>> data_offset =
+ fbb->CreateVector(buffer);
+ const struct timespec timestamp_t =
+ aos::time::to_timespec(aos::monotonic_clock::time_point(
+ std::chrono::nanoseconds(raw_image->monotonic_timestamp_ns())));
+ const foxglove::Time time{static_cast<uint32_t>(timestamp_t.tv_sec),
+ static_cast<uint32_t>(timestamp_t.tv_nsec)};
+ const flatbuffers::Offset<flatbuffers::String> format_offset =
+ fbb->CreateString(format);
+ foxglove::CompressedImage::Builder builder(*fbb);
+ builder.add_timestamp(&time);
+ builder.add_data(data_offset);
+ builder.add_format(format_offset);
+ return builder.Finish();
+}
+
+FoxgloveImageConverter::FoxgloveImageConverter(aos::EventLoop *event_loop,
+ std::string_view input_channel,
+ std::string_view output_channel,
+ ImageCompression compression)
+ : event_loop_(event_loop),
+ sender_(
+ event_loop_->MakeSender<foxglove::CompressedImage>(output_channel)) {
+ event_loop_->MakeWatcher(input_channel, [this, compression](
+ const CameraImage &image) {
+ auto builder = sender_.MakeBuilder();
+ builder.CheckOk(
+ builder.Send(CompressImage(&image, builder.fbb(), compression)));
+ });
+}
+} // namespace frc971::vision
diff --git a/frc971/vision/foxglove_image_converter.h b/frc971/vision/foxglove_image_converter.h
new file mode 100644
index 0000000..add83a6
--- /dev/null
+++ b/frc971/vision/foxglove_image_converter.h
@@ -0,0 +1,36 @@
+#ifndef FRC971_VISION_FOXGLOVE_IMAGE_CONVERTER_H_
+#define FRC971_VISION_FOXGLOVE_IMAGE_CONVERTER_H_
+#include "external/com_github_foxglove_schemas/CompressedImage_generated.h"
+#include "frc971/vision/vision_generated.h"
+#include "aos/events/event_loop.h"
+
+namespace frc971::vision {
+// Empirically, from 2022 logs:
+// PNG is an ~2x space savings relative to raw images.
+// JPEG is an ~10x space savings relative to PNG.
+// Both perform significantly better than attempting to perform in-browser
+// conversion with a user-script in Foxglove Studio.
+enum class ImageCompression { kJpeg, kPng };
+
+flatbuffers::Offset<foxglove::CompressedImage> CompressImage(
+ const CameraImage *raw_image, flatbuffers::FlatBufferBuilder *fbb,
+ ImageCompression compression);
+
+// This class provides a simple converter that will take an AOS CameraImage
+// channel and output
+class FoxgloveImageConverter {
+ public:
+ // Watches for frc971.vision.CameraImage messages on the input_channel and
+ // sends foxglove.CompressedImage messages on the output_channel, using the
+ // specified image compression algorithm.
+ FoxgloveImageConverter(aos::EventLoop *event_loop,
+ std::string_view input_channel,
+ std::string_view output_channel,
+ ImageCompression compression);
+
+ private:
+ aos::EventLoop *event_loop_;
+ aos::Sender<foxglove::CompressedImage> sender_;
+};
+} // namespace frc971::vision
+#endif // FRC971_VISION_FOXGLOVE_IMAGE_CONVERTER_H_