[wpiutil] Move RawFrame to wpiutil; add generation of RawFrame for AprilTags (#5923)

wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java

@@ -0,0 +1,155 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import java.nio.ByteBuffer;
+
+/**
+ * Class for storing raw frame data between image read call.
+ *
+ * <p>Data is reused for each frame read, rather then reallocating every frame.
+ */
+public class RawFrame implements AutoCloseable {
+  private final long m_framePtr;
+  private ByteBuffer m_dataByteBuffer;
+  private long m_dataPtr;
+  private int m_totalData;
+  private int m_width;
+  private int m_height;
+  private int m_pixelFormat;
+
+  /** Construct a new RawFrame. */
+  public RawFrame() {
+    m_framePtr = WPIUtilJNI.allocateRawFrame();
+  }
+
+  /**
+   * Close the RawFrame, releasing native resources. Any images currently using the data will be
+   * invalidated.
+   */
+  @Override
+  public void close() {
+    WPIUtilJNI.freeRawFrame(m_framePtr);
+  }
+
+  /**
+   * Called from JNI to set data in class.
+   *
+   * @param dataByteBuffer A ByteBuffer pointing to the frame data.
+   * @param dataPtr A long (a char* in native code) pointing to the frame data.
+   * @param totalData The total length of the data stored in the frame.
+   * @param width The width of the frame.
+   * @param height The height of the frame.
+   * @param pixelFormat The PixelFormat of the frame.
+   */
+  public void setData(
+      ByteBuffer dataByteBuffer,
+      long dataPtr,
+      int totalData,
+      int width,
+      int height,
+      int pixelFormat) {
+    m_dataByteBuffer = dataByteBuffer;
+    m_dataPtr = dataPtr;
+    m_totalData = totalData;
+    m_width = width;
+    m_height = height;
+    m_pixelFormat = pixelFormat;
+  }
+
+  /**
+   * Get the pointer to native representation of this frame.
+   *
+   * @return The pointer to native representation of this frame.
+   */
+  public long getFramePtr() {
+    return m_framePtr;
+  }
+
+  /**
+   * Get a ByteBuffer pointing to the frame data. This ByteBuffer is backed by the frame directly.
+   * Its lifetime is controlled by the frame. If a new frame gets read, it will overwrite the
+   * current one.
+   *
+   * @return A ByteBuffer pointing to the frame data.
+   */
+  public ByteBuffer getDataByteBuffer() {
+    return m_dataByteBuffer;
+  }
+
+  /**
+   * Get a long (is a char* in native code) pointing to the frame data. This pointer is backed by
+   * the frame directly. Its lifetime is controlled by the frame. If a new frame gets read, it will
+   * overwrite the current one.
+   *
+   * @return A long pointing to the frame data.
+   */
+  public long getDataPtr() {
+    return m_dataPtr;
+  }
+
+  /**
+   * Get the total length of the data stored in the frame.
+   *
+   * @return The total length of the data stored in the frame.
+   */
+  public int getTotalData() {
+    return m_totalData;
+  }
+
+  /**
+   * Get the width of the frame.
+   *
+   * @return The width of the frame.
+   */
+  public int getWidth() {
+    return m_width;
+  }
+
+  /**
+   * Set the width of the frame.
+   *
+   * @param width The width of the frame.
+   */
+  public void setWidth(int width) {
+    this.m_width = width;
+  }
+
+  /**
+   * Get the height of the frame.
+   *
+   * @return The height of the frame.
+   */
+  public int getHeight() {
+    return m_height;
+  }
+
+  /**
+   * Set the height of the frame.
+   *
+   * @param height The height of the frame.
+   */
+  public void setHeight(int height) {
+    this.m_height = height;
+  }
+
+  /**
+   * Get the PixelFormat of the frame.
+   *
+   * @return The PixelFormat of the frame.
+   */
+  public int getPixelFormat() {
+    return m_pixelFormat;
+  }
+
+  /**
+   * Set the PixelFormat of the frame.
+   *
+   * @param pixelFormat The PixelFormat of the frame.
+   */
+  public void setPixelFormat(int pixelFormat) {
+    this.m_pixelFormat = pixelFormat;
+  }
+}

wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java

@@ -80,6 +80,10 @@ public class WPIUtilJNI {
 
   public static native boolean releaseSemaphore(int semHandle, int releaseCount);
 
+  public static native long allocateRawFrame();
+
+  public static native void freeRawFrame(long frame);
+
   /**
    * Waits for a handle to be signaled.
    *

wpiutil/src/main/native/cpp/RawFrame.cpp

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/RawFrame.h"
+
+#include <wpi/MemAlloc.h>
+
+extern "C" {
+void WPI_AllocateRawFrameData(WPI_RawFrame* frame, int requestedSize) {
+  if (frame->dataLength >= requestedSize) {
+    return;
+  }
+  if (frame->data) {
+    frame->data =
+        static_cast<char*>(wpi::safe_realloc(frame->data, requestedSize));
+  } else {
+    frame->data = static_cast<char*>(wpi::safe_malloc(requestedSize));
+  }
+  frame->dataLength = requestedSize;
+}
+
+void WPI_FreeRawFrameData(WPI_RawFrame* frame) {
+  if (frame->data) {
+    std::free(frame->data);
+    frame->data = nullptr;
+    frame->dataLength = 0;
+  }
+}
+}  // extern "C"

wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp

@@ -9,6 +9,7 @@
 #include <fmt/format.h>
 
 #include "edu_wpi_first_util_WPIUtilJNI.h"
+#include "wpi/RawFrame.h"
 #include "wpi/Synchronization.h"
 #include "wpi/jni_util.h"
 #include "wpi/timestamp.h"
@@ -317,4 +318,32 @@ Java_edu_wpi_first_util_WPIUtilJNI_waitForObjectsTimeout
   return MakeJIntArray(env, signaled);
 }
 
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    allocateRawFrame
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_allocateRawFrame
+  (JNIEnv*, jclass)
+{
+  wpi::RawFrame* rawFrame = new wpi::RawFrame{};
+  intptr_t rawFrameIntPtr = reinterpret_cast<intptr_t>(rawFrame);
+  return static_cast<jlong>(rawFrameIntPtr);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    freeRawFrame
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_freeRawFrame
+  (JNIEnv*, jclass, jlong rawFrame)
+{
+  wpi::RawFrame* ptr =
+      reinterpret_cast<wpi::RawFrame*>(static_cast<intptr_t>(rawFrame));
+  delete ptr;
+}
+
 }  // extern "C"

wpiutil/src/main/native/include/wpi/RawFrame.h

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#ifndef WPIUTIL_WPI_RAWFRAME_H_
+#define WPIUTIL_WPI_RAWFRAME_H_
+
+/**
+ * Raw Frame
+ */
+typedef struct WPI_RawFrame {  // NOLINT
+  char* data;
+  int dataLength;
+  int pixelFormat;
+  int width;
+  int height;
+  int totalData;
+} WPI_RawFrame;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Pixel formats
+ */
+enum WPI_PixelFormat {
+  WPI_PIXFMT_UNKNOWN = 0,
+  WPI_PIXFMT_MJPEG,
+  WPI_PIXFMT_YUYV,
+  WPI_PIXFMT_RGB565,
+  WPI_PIXFMT_BGR,
+  WPI_PIXFMT_GRAY,
+  WPI_PIXFMT_Y16,
+  WPI_PIXFMT_UYVY
+};
+
+void WPI_AllocateRawFrameData(WPI_RawFrame* frame, int requestedSize);
+void WPI_FreeRawFrameData(WPI_RawFrame* frame);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
+#ifdef __cplusplus
+namespace wpi {
+struct RawFrame : public WPI_RawFrame {
+  RawFrame() {
+    data = nullptr;
+    dataLength = 0;
+    pixelFormat = WPI_PIXFMT_UNKNOWN;
+    width = 0;
+    height = 0;
+    totalData = 0;
+  }
+
+  ~RawFrame() { WPI_FreeRawFrameData(this); }
+};
+}  // namespace wpi
+
+#endif
+
+#endif  // WPIUTIL_WPI_RAWFRAME_H_