[wpiutil,cscore,apriltag] Fix RawFrame

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java

@@ -54,7 +54,9 @@ public String toString() {
    * @return A RawFrame containing the AprilTag image
    */
   public static RawFrame generate16h5AprilTagImage(int id) {
-    return AprilTagJNI.generate16h5AprilTagImage(id);
+    RawFrame frame = new RawFrame();
+    AprilTagJNI.generate16h5AprilTagImage(frame, frame.getNativeObj(), id);
+    return frame;
   }
 
   /**
@@ -64,6 +66,8 @@ public static RawFrame generate16h5AprilTagImage(int id) {
    * @return A RawFrame containing the AprilTag image
    */
   public static RawFrame generate36h11AprilTagImage(int id) {
-    return AprilTagJNI.generate36h11AprilTagImage(id);
+    RawFrame frame = new RawFrame();
+    AprilTagJNI.generate36h11AprilTagImage(frame, frame.getNativeObj(), id);
+    return frame;
   }
 }

apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java

@@ -191,7 +191,7 @@ public static native Transform3d estimatePose(
       double cx,
       double cy);
 
-  public static native RawFrame generate16h5AprilTagImage(int id);
+  public static native void generate16h5AprilTagImage(RawFrame frameObj, jlong frame, int id);
 
-  public static native RawFrame generate36h11AprilTagImage(int id);
+  public static native void generate36h11AprilTagImage(RawFrame frameObj, jlong frame, int id);
 }

apriltag/src/main/native/cpp/AprilTag.cpp

@@ -4,6 +4,8 @@
 
 #include "frc/apriltag/AprilTag.h"
 
+#include <cstring>
+
 #include <wpi/json.h>
 
 #ifdef _WIN32
@@ -20,40 +22,33 @@
 
 using namespace frc;
 
-wpi::RawFrame AprilTag::Generate36h11AprilTagImage(int id) {
-  apriltag_family_t* tagFamily = tag36h11_create();
-  image_u8_t* image = apriltag_to_image(tagFamily, id);
-  wpi::RawFrame markerFrame{};
-  size_t totalDataSize = image->height * image->stride * sizeof(char);
-  markerFrame.data = static_cast<char*>(
-      std::calloc(image->height * image->stride, sizeof(char)));
-  std::memcpy(markerFrame.data, image->buf, totalDataSize);
-  markerFrame.dataLength = image->width;
-  markerFrame.height = image->height;
-  markerFrame.pixelFormat = WPI_PIXFMT_GRAY;
-  markerFrame.width = image->stride;
-  markerFrame.totalData = totalDataSize;
+static bool FamilyToImage(wpi::RawFrame* frame, apriltag_family_t* family,
+                          int id) {
+  image_u8_t* image = apriltag_to_image(family, id);
+  size_t totalDataSize = image->height * image->stride;
+  bool rv = frame->Reserve(totalDataSize);
+  std::memcpy(frame->data, image->buf, totalDataSize);
+  frame->size = totalDataSize;
+  frame->width = image->stride;
+  frame->height = image->height;
+  frame->stride = image->stride;
+  frame->pixelFormat = WPI_PIXFMT_GRAY;
   image_u8_destroy(image);
+  return rv;
+}
+
+bool AprilTag::Generate36h11AprilTagImage(wpi::RawFrame* frame, int id) {
+  apriltag_family_t* tagFamily = tag36h11_create();
+  bool rv = FamilyToImage(frame, tagFamily, id);
   tag36h11_destroy(tagFamily);
-  return markerFrame;
+  return rv;
 }
 
-wpi::RawFrame AprilTag::Generate16h5AprilTagImage(int id) {
+bool AprilTag::Generate16h5AprilTagImage(wpi::RawFrame* frame, int id) {
   apriltag_family_t* tagFamily = tag16h5_create();
-  image_u8_t* image = apriltag_to_image(tagFamily, id);
-  wpi::RawFrame markerFrame{};
-  size_t totalDataSize = image->height * image->stride * sizeof(char);
-  markerFrame.data = static_cast<char*>(
-      std::calloc(image->height * image->stride, sizeof(char)));
-  std::memcpy(markerFrame.data, image->buf, totalDataSize);
-  markerFrame.dataLength = image->width;
-  markerFrame.height = image->height;
-  markerFrame.pixelFormat = WPI_PIXFMT_GRAY;
-  markerFrame.width = image->stride;
-  markerFrame.totalData = totalDataSize;
-  image_u8_destroy(image);
+  bool rv = FamilyToImage(frame, tagFamily, id);
   tag16h5_destroy(tagFamily);
-  return markerFrame;
+  return rv;
 }
 
 void frc::to_json(wpi::json& json, const AprilTag& apriltag) {

apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp

@@ -5,6 +5,9 @@
 #include <cstdio>
 #include <cstring>
 
+#include <jni.h>
+
+#define WPI_RAWFRAME_JNI
 #include <wpi/RawFrame.h>
 #include <wpi/jni_util.h>
 
@@ -319,21 +322,6 @@ static jobject MakeJObject(JNIEnv* env, const AprilTagPoseEstimate& est) {
                         static_cast<jdouble>(est.error2));
 }
 
-static jobject MakeJObject(JNIEnv* env, const wpi::RawFrame& frame) {
-  static jmethodID constructor = env->GetMethodID(rawFrameCls, "<init>", "()V");
-
-  static jmethodID setData =
-      env->GetMethodID(rawFrameCls, "setData", "(Ljava/nio/ByteBuffer;JIIII)V");
-
-  jobject retVal = env->NewObject(rawFrameCls, constructor);
-  env->CallVoidMethod(
-      retVal, setData, env->NewDirectByteBuffer(frame.data, frame.totalData),
-      static_cast<jlong>(reinterpret_cast<intptr_t>(frame.data)),
-      static_cast<jint>(frame.dataLength), static_cast<jint>(frame.width),
-      static_cast<jint>(frame.height), static_cast<jint>(frame.pixelFormat));
-  return retVal;
-}
-
 extern "C" {
 
 /*
@@ -609,25 +597,38 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePose
 /*
  * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
  * Method:    generate16h5AprilTagImage
- * Signature: (I)Ljava/lang/Object;
+ * Signature: (Ledu/wpi/first/util/RawFrame;JI)V
  */
-JNIEXPORT jobject JNICALL
+JNIEXPORT void JNICALL
 Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate16h5AprilTagImage
-  (JNIEnv* env, jclass, jint id)
+  (JNIEnv* env, jclass, jobject frameObj, jlong framePtr, jint id)
 {
-  return MakeJObject(env, AprilTag::Generate16h5AprilTagImage(id));
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+  if (!frame) {
+    nullPointerEx.Throw(env, "frame is null");
+    return;
+  }
+  bool newData = AprilTag::Generate16h5AprilTagImage(frame, id);
+  wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, newData);
 }
 
 /*
  * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
  * Method:    generate36h11AprilTagImage
- * Signature: (I)Ljava/lang/Object;
+ * Signature: (Ledu/wpi/first/util/RawFrame;JI)V
  */
-JNIEXPORT jobject JNICALL
+JNIEXPORT void JNICALL
 Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate36h11AprilTagImage
-  (JNIEnv* env, jclass, jint id)
+  (JNIEnv* env, jclass, jobject frameObj, jlong framePtr, jint id)
 {
-  return MakeJObject(env, AprilTag::Generate36h11AprilTagImage(id));
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+  if (!frame) {
+    nullPointerEx.Throw(env, "frame is null");
+    return;
+  }
+  // function might reallocate
+  bool newData = AprilTag::Generate36h11AprilTagImage(frame, id);
+  wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, newData);
 }
 
 }  // extern "C"

apriltag/src/main/native/include/frc/apriltag/AprilTag.h

@@ -22,8 +22,8 @@ struct WPILIB_DLLEXPORT AprilTag {
    */
   bool operator==(const AprilTag&) const = default;
 
-  static wpi::RawFrame Generate36h11AprilTagImage(int id);
-  static wpi::RawFrame Generate16h5AprilTagImage(int id);
+  static bool Generate36h11AprilTagImage(wpi::RawFrame* frame, int id);
+  static bool Generate16h5AprilTagImage(wpi::RawFrame* frame, int id);
 };
 
 WPILIB_DLLEXPORT

cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java

@@ -182,21 +182,13 @@ public static native boolean setSourceVideoMode(
   //
   // Image Source Functions
   //
+  public static native void putRawSourceFrame(int source, long frame);
+
   public static native void putRawSourceFrameBB(
-      int source, ByteBuffer data, int width, int height, int pixelFormat, int totalData);
-
-  public static native void putRawSourceFrame(
-      int source, long data, int width, int height, int pixelFormat, int totalData);
-
-  public static void putRawSourceFrame(int source, RawFrame raw) {
-    putRawSourceFrame(
-        source,
-        raw.getDataPtr(),
-        raw.getWidth(),
-        raw.getHeight(),
-        raw.getPixelFormat(),
-        raw.getTotalData());
-  }
+      int source, ByteBuffer data, int size, int width, int height, int stride, int pixelFormat);
+
+  public static native void putRawSourceFrameData(
+      int source, long data, int size, int width, int height, int stride, int pixelFormat);
 
   public static native void notifySourceError(int source, String msg);
 
@@ -263,47 +255,9 @@ public static native void setSourceEnumPropertyChoices(
   //
   public static native void setSinkDescription(int sink, String description);
 
-  private static native long grabRawSinkFrameImpl(
-      int sink,
-      RawFrame rawFrame,
-      long rawFramePtr,
-      ByteBuffer byteBuffer,
-      int width,
-      int height,
-      int pixelFormat);
-
-  private static native long grabRawSinkFrameTimeoutImpl(
-      int sink,
-      RawFrame rawFrame,
-      long rawFramePtr,
-      ByteBuffer byteBuffer,
-      int width,
-      int height,
-      int pixelFormat,
-      double timeout);
-
-  public static long grabSinkFrame(int sink, RawFrame rawFrame) {
-    return grabRawSinkFrameImpl(
-        sink,
-        rawFrame,
-        rawFrame.getFramePtr(),
-        rawFrame.getDataByteBuffer(),
-        rawFrame.getWidth(),
-        rawFrame.getHeight(),
-        rawFrame.getPixelFormat());
-  }
+  public static native long grabRawSinkFrame(int sink, RawFrame frame, long nativeObj);
 
-  public static long grabSinkFrameTimeout(int sink, RawFrame rawFrame, double timeout) {
-    return grabRawSinkFrameTimeoutImpl(
-        sink,
-        rawFrame,
-        rawFrame.getFramePtr(),
-        rawFrame.getDataByteBuffer(),
-        rawFrame.getWidth(),
-        rawFrame.getHeight(),
-        rawFrame.getPixelFormat(),
-        timeout);
-  }
+  public static native long grabRawSinkFrameTimeout(int sink, RawFrame frame, long nativeObj, double timeout);
 
   public static native String getSinkError(int sink);
 

cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java

@@ -33,7 +33,7 @@ public RawSink(String name) {
    * @return Frame time, or 0 on error (call getError() to obtain the error message); the frame time
    *     is in the same time base as wpi::Now(), and is in 1 us increments.
    */
-  protected long grabFrame(RawFrame frame) {
+  public long grabFrame(RawFrame frame) {
     return grabFrame(frame, 0.225);
   }
 
@@ -46,8 +46,8 @@ protected long grabFrame(RawFrame frame) {
    * @return Frame time, or 0 on error (call getError() to obtain the error message); the frame time
    *     is in the same time base as wpi::Now(), and is in 1 us increments.
    */
-  protected long grabFrame(RawFrame frame, double timeout) {
-    return CameraServerJNI.grabSinkFrameTimeout(m_handle, frame, timeout);
+  public long grabFrame(RawFrame frame, double timeout) {
+    return CameraServerJNI.grabRawSinkFrameTimeout(m_handle, frame, frame.getNativeObj(), timeout);
   }
 
   /**
@@ -58,7 +58,7 @@ protected long grabFrame(RawFrame frame, double timeout) {
    * @return Frame time, or 0 on error (call getError() to obtain the error message); the frame time
    *     is in the same time base as wpi::Now(), and is in 1 us increments.
    */
-  protected long grabFrameNoTimeout(RawFrame frame) {
-    return CameraServerJNI.grabSinkFrame(m_handle, frame);
+  public long grabFrameNoTimeout(RawFrame frame) {
+    return CameraServerJNI.grabRawSinkFrame(m_handle, frame, frame.getNativeObj());
   }
 }

cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.cscore.VideoMode;
 import edu.wpi.first.util.PixelFormat;
 import edu.wpi.first.util.RawFrame;
+import java.nio.ByteBuffer;
 
 /**
  * A source for user code to provide video frames as raw bytes.
@@ -46,35 +47,39 @@ public RawSource(String name, PixelFormat pixelFormat, int width, int height, in
    *
    * @param image raw frame image
    */
-  protected void putFrame(RawFrame image) {
-    CameraServerJNI.putRawSourceFrame(m_handle, image);
+  public void putFrame(RawFrame image) {
+    CameraServerJNI.putRawSourceFrame(m_handle, image.getNativeObj());
   }
 
   /**
    * Put a raw image and notify sinks.
    *
-   * @param data raw frame data pointer
+   * @param data raw frame native data pointer
+   * @param size total size in bytes
    * @param width frame width
    * @param height frame height
+   * @param stride size of each row in bytes
    * @param pixelFormat pixel format
-   * @param totalData length of data in total
    */
-  protected void putFrame(long data, int width, int height, int pixelFormat, int totalData) {
-    CameraServerJNI.putRawSourceFrame(m_handle, data, width, height, pixelFormat, totalData);
+  protected void putFrame(long data, int size, int width, int height, int stride, PixelFormat pixelFormat) {
+    CameraServerJNI.putRawSourceFrameData(m_handle, data, size, width, height, stride, pixelFormat.getValue());
   }
 
   /**
    * Put a raw image and notify sinks.
    *
-   * @param data raw frame data pointer
+   * @param data raw frame native ByteBuffer
    * @param width frame width
    * @param height frame height
+   * @param stride size of each row in bytes
    * @param pixelFormat pixel format
-   * @param totalData length of data in total
    */
-  protected void putFrame(
-      long data, int width, int height, PixelFormat pixelFormat, int totalData) {
-    CameraServerJNI.putRawSourceFrame(
-        m_handle, data, width, height, pixelFormat.getValue(), totalData);
+  public void putFrame(
+      ByteBuffer data, int width, int height, int stride, PixelFormat pixelFormat) {
+    if (!data.isDirect()) {
+      throw new UnsupportedOperationException("ByteBuffer must be direct");
+    }
+    CameraServerJNI.putRawSourceFrameBB(
+        m_handle, data, data.limit(), width, height, stride, pixelFormat.getValue());
   }
 }

cscore/src/main/native/cpp/Image.h

@@ -72,6 +72,23 @@ class Image {
     return cv::Mat{height, width, type, m_data.data()};
   }
 
+  int GetStride() const {
+    switch (pixelFormat) {
+      case VideoMode::kYUYV:
+      case VideoMode::kRGB565:
+      case VideoMode::kY16:
+      case VideoMode::kUYVY:
+        return 2 * width;
+      case VideoMode::kBGR:
+        return 3 * width;
+      case VideoMode::kGray:
+        return width;
+      case VideoMode::kMJPEG:
+      default:
+        return 0;
+    }
+  }
+
   cv::_InputArray AsInputArray() { return cv::_InputArray{m_data}; }
 
   bool Is(int width_, int height_) {

cscore/src/main/native/cpp/RawSinkImpl.cpp

@@ -109,9 +109,10 @@ uint64_t RawSinkImpl::GrabFrameImpl(WPI_RawFrame& rawFrame,
   WPI_AllocateRawFrameData(&rawFrame, newImage->size());
   rawFrame.height = newImage->height;
   rawFrame.width = newImage->width;
+  rawFrame.stride = newImage->GetStride();
   rawFrame.pixelFormat = newImage->pixelFormat;
-  rawFrame.totalData = newImage->size();
-  std::copy(newImage->data(), newImage->data() + rawFrame.totalData,
+  rawFrame.size = newImage->size();
+  std::copy(newImage->data(), newImage->data() + rawFrame.size,
             rawFrame.data);
 
   return incomingFrame.GetTime();