[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, long frame, int id);
 
-  public static native RawFrame generate36h11AprilTagImage(int id);
+  public static native void generate36h11AprilTagImage(RawFrame frameObj, long 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->width;
+  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

@@ -2,9 +2,12 @@
 // 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 <jni.h>
+
 #include <cstdio>
 #include <cstring>
 
+#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: (Ljava/lang/Object;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: (Ljava/lang/Object;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

apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java

@@ -0,0 +1,34 @@
+// 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.apriltag;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.util.PixelFormat;
+import org.junit.jupiter.api.Test;
+
+class AprilTagGenerationTest {
+  @Test
+  void test36h11() {
+    var frame = AprilTag.generate36h11AprilTagImage(1);
+    assertEquals(PixelFormat.kGray, frame.getPixelFormat());
+    assertEquals(10, frame.getWidth());
+    assertEquals(10, frame.getHeight());
+    assertEquals(96, frame.getStride());  // default stride in apriltag library
+    assertEquals(960, frame.getSize());
+    // check the diagonal values
+    var data = frame.getData();
+    assertEquals(-1, data.get(96 * 0 + 0));  // outer border is white
+    assertEquals(0, data.get(96 * 1 + 1));   // inner border is black
+    assertEquals(-1, data.get(96 * 2 + 2));
+    assertEquals(-1, data.get(96 * 3 + 3));
+    assertEquals(-1, data.get(96 * 4 + 4));
+    assertEquals(0, data.get(96 * 5 + 5));
+    assertEquals(0, data.get(96 * 6 + 6));
+    assertEquals(-1, data.get(96 * 7 + 7));
+    assertEquals(0, data.get(96 * 8 + 8));   // inner border
+    assertEquals(-1, data.get(96 * 9 + 9));  // outer border
+  }
+}

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());
   }
 }