From 8169da9aebdd8713702cbff4db2535e7c81c46ea Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 26 Dec 2023 10:21:31 -0800 Subject: [PATCH 1/9] [wpiutil,cscore,apriltag] Fix RawFrame --- .../java/edu/wpi/first/apriltag/AprilTag.java | 8 +- .../wpi/first/apriltag/jni/AprilTagJNI.java | 4 +- apriltag/src/main/native/cpp/AprilTag.cpp | 51 +++--- .../src/main/native/cpp/jni/AprilTagJNI.cpp | 47 ++--- .../native/include/frc/apriltag/AprilTag.h | 4 +- .../edu/wpi/first/cscore/CameraServerJNI.java | 62 +------ .../edu/wpi/first/cscore/raw/RawSink.java | 10 +- .../edu/wpi/first/cscore/raw/RawSource.java | 29 ++-- cscore/src/main/native/cpp/Image.h | 17 ++ cscore/src/main/native/cpp/RawSinkImpl.cpp | 5 +- cscore/src/main/native/cpp/RawSourceImpl.cpp | 5 +- .../main/native/cpp/jni/CameraServerJNI.cpp | 145 ++++++++-------- .../java/edu/wpi/first/util/RawFrame.java | 160 +++++++++++------- .../java/edu/wpi/first/util/WPIUtilJNI.java | 11 +- wpiutil/src/main/native/cpp/RawFrame.cpp | 43 +++-- .../src/main/native/cpp/jni/WPIUtilJNI.cpp | 75 +++++++- .../src/main/native/include/wpi/RawFrame.h | 119 ++++++++++--- 17 files changed, 492 insertions(+), 303 deletions(-) diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java index 880c803db54..2c9be4a6866 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java +++ b/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; } } diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java b/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java index f6e585f10d5..f6d393aead6 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java +++ b/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); } diff --git a/apriltag/src/main/native/cpp/AprilTag.cpp b/apriltag/src/main/native/cpp/AprilTag.cpp index d5f5e8fe249..12b5b7173b4 100644 --- a/apriltag/src/main/native/cpp/AprilTag.cpp +++ b/apriltag/src/main/native/cpp/AprilTag.cpp @@ -4,6 +4,8 @@ #include "frc/apriltag/AprilTag.h" +#include + #include #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( - 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( - 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) { diff --git a/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp b/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp index 5168661c073..e894d7af1de 100644 --- a/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp +++ b/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp @@ -5,6 +5,9 @@ #include #include +#include + +#define WPI_RAWFRAME_JNI #include #include @@ -319,21 +322,6 @@ static jobject MakeJObject(JNIEnv* env, const AprilTagPoseEstimate& est) { static_cast(est.error2)); } -static jobject MakeJObject(JNIEnv* env, const wpi::RawFrame& frame) { - static jmethodID constructor = env->GetMethodID(rawFrameCls, "", "()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(reinterpret_cast(frame.data)), - static_cast(frame.dataLength), static_cast(frame.width), - static_cast(frame.height), static_cast(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(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(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" diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTag.h b/apriltag/src/main/native/include/frc/apriltag/AprilTag.h index 6449c1e4402..abb8ac59371 100644 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTag.h +++ b/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 diff --git a/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java b/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java index 54c4d321ed6..bed47ceb511 100644 --- a/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java +++ b/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); diff --git a/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java b/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java index 5d4bb1a5ead..babb0fdede6 100644 --- a/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java +++ b/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()); } } diff --git a/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java b/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java index 252f7a6efb1..6a74f4f5ec2 100644 --- a/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java +++ b/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()); } } diff --git a/cscore/src/main/native/cpp/Image.h b/cscore/src/main/native/cpp/Image.h index 83cbe899747..b66a7f17421 100644 --- a/cscore/src/main/native/cpp/Image.h +++ b/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_) { diff --git a/cscore/src/main/native/cpp/RawSinkImpl.cpp b/cscore/src/main/native/cpp/RawSinkImpl.cpp index 877de1d5ed2..73c2377d20b 100644 --- a/cscore/src/main/native/cpp/RawSinkImpl.cpp +++ b/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(); diff --git a/cscore/src/main/native/cpp/RawSourceImpl.cpp b/cscore/src/main/native/cpp/RawSourceImpl.cpp index eab9a7b4de8..0d53f8993b7 100644 --- a/cscore/src/main/native/cpp/RawSourceImpl.cpp +++ b/cscore/src/main/native/cpp/RawSourceImpl.cpp @@ -39,10 +39,11 @@ void RawSourceImpl::PutFrame(const WPI_RawFrame& image) { type = CV_8UC1; break; } - cv::Mat finalImage{image.height, image.width, type, image.data}; + cv::Mat finalImage{image.height, image.width, type, image.data, + static_cast(image.stride)}; std::unique_ptr dest = AllocImage(static_cast(image.pixelFormat), - image.width, image.height, image.totalData); + image.width, image.height, image.size); finalImage.copyTo(dest->AsMat()); SourceImpl::PutFrame(std::move(dest), wpi::Now()); diff --git a/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp b/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp index 95b6922fa81..edd0b062ede 100644 --- a/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp +++ b/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp @@ -7,6 +7,8 @@ #include #include +#define WPI_RAWFRAME_JNI +#include #include #include @@ -1220,48 +1222,78 @@ Java_edu_wpi_first_cscore_CameraServerCvJNI_putSourceFrame } } -// int width, int height, int pixelFormat, int totalData +/* + * Class: edu_wpi_first_cscore_CameraServerJNI + * Method: putRawSourceFrame + * Signature: (IJ)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrame + (JNIEnv* env, jclass, jint source, jlong framePtr) +{ + auto* frame = reinterpret_cast(framePtr); + if (!frame) { + nullPointerEx.Throw(env, "frame is null"); + return; + } + CS_Status status = 0; + cs::PutSourceFrame(source, *frame, &status); + CheckStatus(env, status); +} /* * Class: edu_wpi_first_cscore_CameraServerJNI * Method: putRawSourceFrameBB - * Signature: (ILjava/lang/Object;IIII)V + * Signature: (ILjava/lang/Object;IIIII)V */ JNIEXPORT void JNICALL Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrameBB - (JNIEnv* env, jclass, jint source, jobject byteBuffer, jint width, - jint height, jint pixelFormat, jint totalData) + (JNIEnv* env, jclass, jint source, jobject data, jint size, jint width, + jint height, jint stride, jint pixelFormat) { - WPI_RawFrame rawFrame; - rawFrame.data = - reinterpret_cast(env->GetDirectBufferAddress(byteBuffer)); - rawFrame.totalData = totalData; - rawFrame.pixelFormat = pixelFormat; - rawFrame.width = width; - rawFrame.height = height; + WPI_RawFrame frame; // use WPI_Frame because we don't want the destructor + frame.data = static_cast(env->GetDirectBufferAddress(data)); + if (!frame.data) { + nullPointerEx.Throw(env, "data is null"); + return; + } + frame.freeFunc = nullptr; + frame.freeCbData = nullptr; + frame.size = size; + frame.width = width; + frame.height = height; + frame.stride = stride; + frame.pixelFormat = pixelFormat; CS_Status status = 0; - cs::PutSourceFrame(source, rawFrame, &status); + cs::PutSourceFrame(source, frame, &status); CheckStatus(env, status); } /* * Class: edu_wpi_first_cscore_CameraServerJNI - * Method: putRawSourceFrame - * Signature: (IJIIII)V + * Method: putRawSourceFrameData + * Signature: (IJIIIII)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrame - (JNIEnv* env, jclass, jint source, jlong ptr, jint width, jint height, - jint pixelFormat, jint totalData) -{ - WPI_RawFrame rawFrame; - rawFrame.data = reinterpret_cast(static_cast(ptr)); - rawFrame.totalData = totalData; - rawFrame.pixelFormat = pixelFormat; - rawFrame.width = width; - rawFrame.height = height; +Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrameData + (JNIEnv* env, jclass, jint source, jlong data, jint size, jint width, jint height, + jint stride, jint pixelFormat) +{ + WPI_RawFrame frame; // use WPI_Frame because we don't want the destructor + frame.data = reinterpret_cast(data); + if (!frame.data) { + nullPointerEx.Throw(env, "data is null"); + return; + } + frame.freeFunc = nullptr; + frame.freeCbData = nullptr; + frame.size = size; + frame.width = width; + frame.height = height; + frame.stride = stride; + frame.pixelFormat = pixelFormat; CS_Status status = 0; - cs::PutSourceFrame(source, rawFrame, &status); + cs::PutSourceFrame(source, frame, &status); CheckStatus(env, status); } @@ -1722,72 +1754,45 @@ Java_edu_wpi_first_cscore_CameraServerCvJNI_grabSinkFrameTimeout } } -static void SetRawFrameData(JNIEnv* env, jobject rawFrameObj, - jobject byteBuffer, bool didChangeDataPtr, - const WPI_RawFrame& frame) { - static jmethodID setMethod = - env->GetMethodID(rawFrameCls, "setData", "(Ljava/nio/ByteBuffer;JIIII)V"); - jlong framePtr = static_cast(reinterpret_cast(frame.data)); - - if (didChangeDataPtr) { - byteBuffer = env->NewDirectByteBuffer(frame.data, frame.dataLength); - } - - env->CallVoidMethod( - rawFrameObj, setMethod, byteBuffer, framePtr, - static_cast(frame.totalData), static_cast(frame.width), - static_cast(frame.height), static_cast(frame.pixelFormat)); -} - /* * Class: edu_wpi_first_cscore_CameraServerJNI - * Method: grabRawSinkFrameImpl - * Signature: (ILjava/lang/Object;JLjava/lang/Object;III)J + * Method: grabRawSinkFrame + * Signature: (ILjava/lang/Object;J)J */ JNIEXPORT jlong JNICALL -Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrameImpl - (JNIEnv* env, jclass, jint sink, jobject rawFrameObj, jlong rawFramePtr, - jobject byteBuffer, jint width, jint height, jint pixelFormat) -{ - WPI_RawFrame* ptr = - reinterpret_cast(static_cast(rawFramePtr)); - auto origDataPtr = ptr->data; - ptr->width = width; - ptr->height = height; - ptr->pixelFormat = pixelFormat; - CS_Status status = 0; - auto rv = cs::GrabSinkFrame(static_cast(sink), *ptr, &status); +Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrame + (JNIEnv* env, jclass, jint sink, jobject frameObj, jlong framePtr) +{ + auto* frame = reinterpret_cast(framePtr); + auto origData = frame->data; + CS_Status status = 0; + auto rv = cs::GrabSinkFrame(static_cast(sink), *frame, &status); if (!CheckStatus(env, status)) { return 0; } - SetRawFrameData(env, rawFrameObj, byteBuffer, origDataPtr != ptr->data, *ptr); + wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, origData != frame->data); return rv; } /* * Class: edu_wpi_first_cscore_CameraServerJNI - * Method: grabRawSinkFrameTimeoutImpl - * Signature: (ILjava/lang/Object;JLjava/lang/Object;IIID)J + * Method: grabRawSinkFrameTimeout + * Signature: (ILjava/lang/Object;JD)J */ JNIEXPORT jlong JNICALL -Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrameTimeoutImpl - (JNIEnv* env, jclass, jint sink, jobject rawFrameObj, jlong rawFramePtr, - jobject byteBuffer, jint width, jint height, jint pixelFormat, +Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrameTimeout + (JNIEnv* env, jclass, jint sink, jobject frameObj, jlong framePtr, jdouble timeout) { - WPI_RawFrame* ptr = - reinterpret_cast(static_cast(rawFramePtr)); - auto origDataPtr = ptr->data; - ptr->width = width; - ptr->height = height; - ptr->pixelFormat = pixelFormat; + auto* frame = reinterpret_cast(framePtr); + auto origData = frame->data; CS_Status status = 0; - auto rv = cs::GrabSinkFrameTimeout(static_cast(sink), *ptr, timeout, + auto rv = cs::GrabSinkFrameTimeout(static_cast(sink), *frame, timeout, &status); if (!CheckStatus(env, status)) { return 0; } - SetRawFrameData(env, rawFrameObj, byteBuffer, origDataPtr != ptr->data, *ptr); + wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, origData != frame->data); return rv; } diff --git a/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java b/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java index 19aeca072c0..658232fdbc9 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java @@ -12,17 +12,16 @@ *

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 long m_nativeObj; + private ByteBuffer m_data; private int m_width; private int m_height; - private int m_pixelFormat; + private int m_stride; + private PixelFormat m_pixelFormat; - /** Construct a new RawFrame. */ + /** Construct a new empty RawFrame. */ public RawFrame() { - m_framePtr = WPIUtilJNI.allocateRawFrame(); + m_nativeObj = WPIUtilJNI.allocateRawFrame(); } /** @@ -31,32 +30,95 @@ public RawFrame() { */ @Override public void close() { - WPIUtilJNI.freeRawFrame(m_framePtr); + WPIUtilJNI.freeRawFrame(m_nativeObj); + m_nativeObj = 0; } /** * 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. + * @param data A native ByteBuffer pointing to the frame data. + * @param width The width of the frame, in pixels + * @param height The height of the frame, in pixels + * @param stride The number of bytes in each row of image data + * @param pixelFormat The PixelFormat of the frame */ - public void setData( - ByteBuffer dataByteBuffer, - long dataPtr, - int totalData, + void setDataJNI( + ByteBuffer data, + int width, + int height, + int stride, + int pixelFormat) { + m_data = data; + m_width = width; + m_height = height; + m_stride = stride; + m_pixelFormat = PixelFormat.getFromInt(pixelFormat); + } + + /** + * Called from JNI to set info in class. + * + * @param width The width of the frame, in pixels + * @param height The height of the frame, in pixels + * @param stride The number of bytes in each row of image data + * @param pixelFormat The PixelFormat of the frame + */ + void setInfoJNI( int width, int height, + int stride, int pixelFormat) { - m_dataByteBuffer = dataByteBuffer; - m_dataPtr = dataPtr; - m_totalData = totalData; m_width = width; m_height = height; + m_stride = stride; + m_pixelFormat = PixelFormat.getFromInt(pixelFormat); + } + + /** + * Set frame data. + * + * @param data A native ByteBuffer pointing to the frame data. + * @param width The width of the frame, in pixels + * @param height The height of the frame, in pixels + * @param stride The number of bytes in each row of image data + * @param pixelFormat The PixelFormat of the frame + */ + public void setData( + ByteBuffer data, + int width, + int height, + int stride, + PixelFormat pixelFormat) { + if (!data.isDirect()) { + throw new UnsupportedOperationException("ByteBuffer must be direct"); + } + m_data = data; + m_width = width; + m_height = height; + m_stride = stride; m_pixelFormat = pixelFormat; + WPIUtilJNI.setRawFrameData(m_nativeObj, data, data.limit(), width, height, stride, pixelFormat.getValue()); + } + + /** + * Call to set frame information. + * + * @param width The width of the frame, in pixels + * @param height The height of the frame, in pixels + * @param stride The number of bytes in each row of image data + * @param pixelFormat The PixelFormat of the frame + */ + public void setInfo( + int width, + int height, + int stride, + PixelFormat pixelFormat) { + m_width = width; + m_height = height; + m_stride = stride; + m_pixelFormat = pixelFormat; + WPIUtilJNI.setRawFrameInfo(m_nativeObj, m_data.limit(), width, height, stride, pixelFormat.getValue()); } /** @@ -64,8 +126,8 @@ public void setData( * * @return The pointer to native representation of this frame. */ - public long getFramePtr() { - return m_framePtr; + public long getNativeObj() { + return m_nativeObj; } /** @@ -75,64 +137,55 @@ public long getFramePtr() { * * @return A ByteBuffer pointing to the frame data. */ - public ByteBuffer getDataByteBuffer() { - return m_dataByteBuffer; + public ByteBuffer getData() { + return m_data; } /** - * Get a long (is a char* in native code) pointing to the frame data. This pointer is backed by + * Get a long (is a uint8_t* 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; + return WPIUtilJNI.getRawFrameDataPtr(m_nativeObj); } /** - * Get the total length of the data stored in the frame. + * Get the total size of the data stored in the frame, in bytes. * - * @return The total length of the data stored in the frame. + * @return The total size of the data stored in the frame. */ - public int getTotalData() { - return m_totalData; + public int getSize() { + return m_data.limit(); } /** - * Get the width of the frame. + * Get the width of the image. * - * @return The width of the frame. + * @return The width of the image, in pixels. */ public int getWidth() { return m_width; } /** - * Set the width of the frame. + * Get the height of the image. * - * @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. + * @return The height of the image, in pixels. */ public int getHeight() { return m_height; } /** - * Set the height of the frame. + * Get the number of bytes in each row of image data. * - * @param height The height of the frame. + * @return The image data stride, in bytes. */ - public void setHeight(int height) { - this.m_height = height; + public int getStride() { + return m_stride; } /** @@ -140,16 +193,7 @@ public void setHeight(int height) { * * @return The PixelFormat of the frame. */ - public int getPixelFormat() { + public PixelFormat 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; - } } diff --git a/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java b/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java index 8718324ebe1..a2fb5cb2011 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java @@ -5,6 +5,7 @@ package edu.wpi.first.util; import java.io.IOException; +import java.nio.ByteBuffer; import java.util.concurrent.atomic.AtomicBoolean; public class WPIUtilJNI { @@ -80,9 +81,15 @@ public static synchronized void forceLoad() throws IOException { public static native boolean releaseSemaphore(int semHandle, int releaseCount); - public static native long allocateRawFrame(); + static native long allocateRawFrame(); - public static native void freeRawFrame(long frame); + static native void freeRawFrame(long frame); + + static native long getRawFrameDataPtr(long frame); + + static native void setRawFrameData(long frame, ByteBuffer data, int size, int width, int height, int stride, int pixelFormat); + + static native void setRawFrameInfo(long frame, int size, int width, int height, int stride, int pixelFormat); /** * Waits for a handle to be signaled. diff --git a/wpiutil/src/main/native/cpp/RawFrame.cpp b/wpiutil/src/main/native/cpp/RawFrame.cpp index a97a2ccdfcc..6688a27788d 100644 --- a/wpiutil/src/main/native/cpp/RawFrame.cpp +++ b/wpiutil/src/main/native/cpp/RawFrame.cpp @@ -4,27 +4,46 @@ #include "wpi/RawFrame.h" +#include + #include extern "C" { -void WPI_AllocateRawFrameData(WPI_RawFrame* frame, int requestedSize) { - if (frame->dataLength >= requestedSize) { - return; - } - if (frame->data) { - frame->data = - static_cast(wpi::safe_realloc(frame->data, requestedSize)); - } else { - frame->data = static_cast(wpi::safe_malloc(requestedSize)); +int WPI_AllocateRawFrameData(WPI_RawFrame* frame, size_t requestedSize) { + if (frame->capacity >= requestedSize) { + return 0; } - frame->dataLength = requestedSize; + WPI_FreeRawFrameData(frame); + frame->data = static_cast(wpi::safe_malloc(requestedSize)); + frame->capacity = requestedSize; + frame->size = 0; + return 1; } void WPI_FreeRawFrameData(WPI_RawFrame* frame) { if (frame->data) { - std::free(frame->data); + if (frame->freeFunc) { + frame->freeFunc(frame->freeCbData, frame->data, frame->capacity); + } else { + std::free(frame->data); + } frame->data = nullptr; - frame->dataLength = 0; + frame->freeFunc = nullptr; + frame->freeCbData = nullptr; + frame->capacity = 0; } } + +void WPI_SetRawFrameData(WPI_RawFrame* frame, void* data, size_t size, + size_t capacity, void* cbdata, + void (*freeFunc)(void* cbdata, void* data, + size_t capacity)) { + WPI_FreeRawFrameData(frame); + frame->data = static_cast(data); + frame->freeFunc = freeFunc; + frame->freeCbData = cbdata; + frame->capacity = capacity; + frame->size = size; +} + } // extern "C" diff --git a/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp b/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp index ad0e1a45955..cd32c3351a6 100644 --- a/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp +++ b/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp @@ -327,9 +327,7 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_first_util_WPIUtilJNI_allocateRawFrame (JNIEnv*, jclass) { - wpi::RawFrame* rawFrame = new wpi::RawFrame{}; - intptr_t rawFrameIntPtr = reinterpret_cast(rawFrame); - return static_cast(rawFrameIntPtr); + return reinterpret_cast(new wpi::RawFrame); } /* @@ -339,11 +337,74 @@ Java_edu_wpi_first_util_WPIUtilJNI_allocateRawFrame */ JNIEXPORT void JNICALL Java_edu_wpi_first_util_WPIUtilJNI_freeRawFrame - (JNIEnv*, jclass, jlong rawFrame) + (JNIEnv*, jclass, jlong frame) { - wpi::RawFrame* ptr = - reinterpret_cast(static_cast(rawFrame)); - delete ptr; + delete reinterpret_cast(frame); +} + +/* + * Class: edu_wpi_first_util_WPIUtilJNI + * Method: getRawFrameDataPtr + * Signature: (J)J + */ +JNIEXPORT jlong JNICALL +Java_edu_wpi_first_util_WPIUtilJNI_getRawFrameDataPtr + (JNIEnv* env, jclass, jlong frame) +{ + auto* f = reinterpret_cast(frame); + if (!f) { + wpi::ThrowNullPointerException(env, "frame is null"); + return 0; + } + return reinterpret_cast(f->data); +} + +/* + * Class: edu_wpi_first_util_WPIUtilJNI + * Method: setRawFrameData + * Signature: (JLjava/nio/ByteBuffer;IIIII)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameData + (JNIEnv* env, jclass, jlong frame, jobject data, jint size, jint width, jint height, jint stride, jint pixelFormat) +{ + auto* f = reinterpret_cast(frame); + if (!f) { + wpi::ThrowNullPointerException(env, "frame is null"); + return; + } + auto buf = env->GetDirectBufferAddress(data); + if (!buf) { + wpi::ThrowNullPointerException(env, "data is null"); + return; + } + // there's no way to free a passed-in direct byte buffer + f->SetData(buf, size, env->GetDirectBufferCapacity(data), nullptr, + [](void*, void*, size_t) {}); + f->width = width; + f->height = height; + f->stride = stride; + f->pixelFormat = pixelFormat; +} + +/* + * Class: edu_wpi_first_util_WPIUtilJNI + * Method: setRawFrameInfo + * Signature: (JIIIII)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameInfo + (JNIEnv* env, jclass, jlong frame, jint size, jint width, jint height, jint stride, jint pixelFormat) +{ + auto* f = reinterpret_cast(frame); + if (!f) { + wpi::ThrowNullPointerException(env, "frame is null"); + return; + } + f->width = width; + f->height = height; + f->stride = stride; + f->pixelFormat = pixelFormat; } } // extern "C" diff --git a/wpiutil/src/main/native/include/wpi/RawFrame.h b/wpiutil/src/main/native/include/wpi/RawFrame.h index 5254a64df16..e6ca97916b3 100644 --- a/wpiutil/src/main/native/include/wpi/RawFrame.h +++ b/wpiutil/src/main/native/include/wpi/RawFrame.h @@ -5,38 +5,61 @@ #ifndef WPIUTIL_WPI_RAWFRAME_H_ #define WPIUTIL_WPI_RAWFRAME_H_ +#include + +#ifdef __cplusplus +#include +#include +#else +#include +#endif + +#ifdef WPI_RAWFRAME_JNI +#include "jni_util.h" +#endif + +#ifdef __cplusplus +extern "C" { +#endif + /** * Raw Frame */ typedef struct WPI_RawFrame { // NOLINT - char* data; - int dataLength; - int pixelFormat; - int width; - int height; - int totalData; + // image data + uint8_t* data; + // function to free image data (may be NULL) + void (*freeFunc)(void* cbdata, void* data, size_t capacity); + void* freeCbData; // data passed to freeFunc + size_t capacity; // data buffer capacity, in bytes + size_t size; // actual size of data, in bytes + int pixelFormat; // WPI_PixelFormat + int width; // width of image, in pixels + int height; // height of image, in pixels + int stride; // size of each row of data, in bytes (may be 0) } 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 + WPI_PIXFMT_UNKNOWN = 0, // unknown + WPI_PIXFMT_MJPEG, // Motion-JPEG (compressed image data) + WPI_PIXFMT_YUYV, // YUV 4:2:2, 16 bpp + WPI_PIXFMT_RGB565, // RGB 5-6-5, 16 bpp + WPI_PIXFMT_BGR, // BGR 8-8-8, 24 bpp + WPI_PIXFMT_GRAY, // Grayscale, 8 bpp + WPI_PIXFMT_Y16, // Grayscale, 16 bpp + WPI_PIXFMT_UYVY, // YUV 4:2:2, 16 bpp }; -void WPI_AllocateRawFrameData(WPI_RawFrame* frame, int requestedSize); +// Returns nonzero if the frame data was allocated/reallocated +int WPI_AllocateRawFrameData(WPI_RawFrame* frame, size_t requestedSize); void WPI_FreeRawFrameData(WPI_RawFrame* frame); +void WPI_SetRawFrameData(WPI_RawFrame* frame, void* data, size_t size, + size_t capacity, void* cbdata, + void (*freeFunc)(void* cbdata, void* data, + size_t capacity)); #ifdef __cplusplus } // extern "C" @@ -47,17 +70,69 @@ namespace wpi { struct RawFrame : public WPI_RawFrame { RawFrame() { data = nullptr; - dataLength = 0; + freeFunc = nullptr; + freeCbData = nullptr; + capacity = 0; + size = 0; pixelFormat = WPI_PIXFMT_UNKNOWN; width = 0; height = 0; - totalData = 0; + } + RawFrame(const RawFrame&) = delete; + RawFrame& operator=(const RawFrame&) = delete; + RawFrame(RawFrame&& rhs) noexcept : WPI_RawFrame{rhs} { + rhs.data = nullptr; + rhs.freeFunc = nullptr; + rhs.freeCbData = nullptr; + rhs.capacity = 0; + rhs.size = 0; + } + RawFrame& operator=(RawFrame&& rhs) noexcept { + *static_cast(this) = rhs; + rhs.data = nullptr; + rhs.freeFunc = nullptr; + rhs.freeCbData = nullptr; + rhs.capacity = 0; + rhs.size = 0; + return *this; + } + + void SetData(void* data, size_t size, size_t capacity, void* cbdata, + void (*freeFunc)(void* cbdata, void* data, size_t capacity)) { + WPI_SetRawFrameData(this, data, size, capacity, cbdata, freeFunc); + } + + // returns true if the frame data was allocated/reallocated + bool Reserve(size_t size) { + return WPI_AllocateRawFrameData(this, size) != 0; } ~RawFrame() { WPI_FreeRawFrameData(this); } }; -} // namespace wpi +#ifdef WPI_RAWFRAME_JNI +template T> +void SetFrameData(JNIEnv* env, jclass rawFrameCls, jobject jframe, + const T& frame, bool newData) { + if (newData) { + static jmethodID setData = env->GetMethodID(rawFrameCls, "setDataJNI", + "(Ljava/nio/ByteBuffer;IIII)V"); + env->CallVoidMethod( + jframe, setData, env->NewDirectByteBuffer(frame.data, frame.size), + static_cast(frame.width), static_cast(frame.height), + static_cast(frame.stride), static_cast(frame.pixelFormat)); + } else { + static jmethodID setInfo = + env->GetMethodID(rawFrameCls, "setInfoJNI", "(IIII)V"); + env->CallVoidMethod(jframe, setInfo, static_cast(frame.width), + static_cast(frame.height), + static_cast(frame.stride), + static_cast(frame.pixelFormat)); + } +} +#endif + +} // namespace wpi #endif #endif // WPIUTIL_WPI_RAWFRAME_H_ From be9e408376a22c6adb500f8b558237e0e64b1dc4 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 26 Dec 2023 10:35:50 -0800 Subject: [PATCH 2/9] Formatting --- apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp | 8 ++++---- cscore/src/main/native/cpp/RawSinkImpl.cpp | 3 +-- .../src/main/native/cpp/jni/CameraServerJNI.cpp | 15 +++++++++------ wpiutil/src/main/native/cpp/RawFrame.cpp | 4 ++-- wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp | 8 +++++--- 5 files changed, 21 insertions(+), 17 deletions(-) diff --git a/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp b/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp index e894d7af1de..b68b131087d 100644 --- a/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp +++ b/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp @@ -2,11 +2,11 @@ // 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 + #include #include -#include - #define WPI_RAWFRAME_JNI #include #include @@ -597,7 +597,7 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePose /* * Class: edu_wpi_first_apriltag_jni_AprilTagJNI * Method: generate16h5AprilTagImage - * Signature: (Ledu/wpi/first/util/RawFrame;JI)V + * Signature: (Ljava/lang/Object;JI)V */ JNIEXPORT void JNICALL Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate16h5AprilTagImage @@ -615,7 +615,7 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate16h5AprilTagImage /* * Class: edu_wpi_first_apriltag_jni_AprilTagJNI * Method: generate36h11AprilTagImage - * Signature: (Ledu/wpi/first/util/RawFrame;JI)V + * Signature: (Ljava/lang/Object;JI)V */ JNIEXPORT void JNICALL Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate36h11AprilTagImage diff --git a/cscore/src/main/native/cpp/RawSinkImpl.cpp b/cscore/src/main/native/cpp/RawSinkImpl.cpp index 73c2377d20b..77116416d8c 100644 --- a/cscore/src/main/native/cpp/RawSinkImpl.cpp +++ b/cscore/src/main/native/cpp/RawSinkImpl.cpp @@ -112,8 +112,7 @@ uint64_t RawSinkImpl::GrabFrameImpl(WPI_RawFrame& rawFrame, rawFrame.stride = newImage->GetStride(); rawFrame.pixelFormat = newImage->pixelFormat; rawFrame.size = newImage->size(); - std::copy(newImage->data(), newImage->data() + rawFrame.size, - rawFrame.data); + std::copy(newImage->data(), newImage->data() + rawFrame.size, rawFrame.data); return incomingFrame.GetTime(); } diff --git a/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp b/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp index edd0b062ede..d7a9b4537ea 100644 --- a/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp +++ b/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp @@ -7,6 +7,7 @@ #include #include + #define WPI_RAWFRAME_JNI #include #include @@ -1276,8 +1277,8 @@ Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrameBB */ JNIEXPORT void JNICALL Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrameData - (JNIEnv* env, jclass, jint source, jlong data, jint size, jint width, jint height, - jint stride, jint pixelFormat) + (JNIEnv* env, jclass, jint source, jlong data, jint size, jint width, + jint height, jint stride, jint pixelFormat) { WPI_RawFrame frame; // use WPI_Frame because we don't want the destructor frame.data = reinterpret_cast(data); @@ -1770,7 +1771,8 @@ Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrame if (!CheckStatus(env, status)) { return 0; } - wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, origData != frame->data); + wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, + origData != frame->data); return rv; } @@ -1787,12 +1789,13 @@ Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrameTimeout auto* frame = reinterpret_cast(framePtr); auto origData = frame->data; CS_Status status = 0; - auto rv = cs::GrabSinkFrameTimeout(static_cast(sink), *frame, timeout, - &status); + auto rv = cs::GrabSinkFrameTimeout(static_cast(sink), *frame, + timeout, &status); if (!CheckStatus(env, status)) { return 0; } - wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, origData != frame->data); + wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, + origData != frame->data); return rv; } diff --git a/wpiutil/src/main/native/cpp/RawFrame.cpp b/wpiutil/src/main/native/cpp/RawFrame.cpp index 6688a27788d..2bc36ed1603 100644 --- a/wpiutil/src/main/native/cpp/RawFrame.cpp +++ b/wpiutil/src/main/native/cpp/RawFrame.cpp @@ -4,10 +4,10 @@ #include "wpi/RawFrame.h" -#include - #include +#include + extern "C" { int WPI_AllocateRawFrameData(WPI_RawFrame* frame, size_t requestedSize) { if (frame->capacity >= requestedSize) { diff --git a/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp b/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp index cd32c3351a6..26d1c93a839 100644 --- a/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp +++ b/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp @@ -362,11 +362,12 @@ Java_edu_wpi_first_util_WPIUtilJNI_getRawFrameDataPtr /* * Class: edu_wpi_first_util_WPIUtilJNI * Method: setRawFrameData - * Signature: (JLjava/nio/ByteBuffer;IIIII)V + * Signature: (JLjava/lang/Object;IIIII)V */ JNIEXPORT void JNICALL Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameData - (JNIEnv* env, jclass, jlong frame, jobject data, jint size, jint width, jint height, jint stride, jint pixelFormat) + (JNIEnv* env, jclass, jlong frame, jobject data, jint size, jint width, + jint height, jint stride, jint pixelFormat) { auto* f = reinterpret_cast(frame); if (!f) { @@ -394,7 +395,8 @@ Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameData */ JNIEXPORT void JNICALL Java_edu_wpi_first_util_WPIUtilJNI_setRawFrameInfo - (JNIEnv* env, jclass, jlong frame, jint size, jint width, jint height, jint stride, jint pixelFormat) + (JNIEnv* env, jclass, jlong frame, jint size, jint width, jint height, + jint stride, jint pixelFormat) { auto* f = reinterpret_cast(frame); if (!f) { From 6b188e2ad3071a49facd79390d420b9736c484b0 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 26 Dec 2023 13:23:41 -0800 Subject: [PATCH 3/9] Fix copy-paste --- apriltag/src/main/native/cpp/AprilTag.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apriltag/src/main/native/cpp/AprilTag.cpp b/apriltag/src/main/native/cpp/AprilTag.cpp index 12b5b7173b4..99ac81b49cf 100644 --- a/apriltag/src/main/native/cpp/AprilTag.cpp +++ b/apriltag/src/main/native/cpp/AprilTag.cpp @@ -29,7 +29,7 @@ static bool FamilyToImage(wpi::RawFrame* frame, apriltag_family_t* family, bool rv = frame->Reserve(totalDataSize); std::memcpy(frame->data, image->buf, totalDataSize); frame->size = totalDataSize; - frame->width = image->stride; + frame->width = image->width; frame->height = image->height; frame->stride = image->stride; frame->pixelFormat = WPI_PIXFMT_GRAY; From 4f393c7db622c9c0c60dab8e9f92dbcbbeaec827 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 26 Dec 2023 17:34:40 -0800 Subject: [PATCH 4/9] Fix typo --- .../src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java b/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java index f6d393aead6..d03169b8b96 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java +++ b/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 void generate16h5AprilTagImage(RawFrame frameObj, jlong frame, int id); + public static native void generate16h5AprilTagImage(RawFrame frameObj, long frame, int id); - public static native void generate36h11AprilTagImage(RawFrame frameObj, jlong frame, int id); + public static native void generate36h11AprilTagImage(RawFrame frameObj, long frame, int id); } From 854a378f5a5b3c7a06013ea74989cc0361bf4be5 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 26 Dec 2023 18:00:50 -0800 Subject: [PATCH 5/9] Add test --- .../apriltag/AprilTagGenerationTest.java | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java new file mode 100644 index 00000000000..8d7e847f4dc --- /dev/null +++ b/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 + } +} From c66efd8232b2e12b7668999d0e344aba119043e2 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 26 Dec 2023 18:02:15 -0800 Subject: [PATCH 6/9] Disable linter --- wpiutil/src/main/native/include/wpi/RawFrame.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/wpiutil/src/main/native/include/wpi/RawFrame.h b/wpiutil/src/main/native/include/wpi/RawFrame.h index e6ca97916b3..904cc1eaf44 100644 --- a/wpiutil/src/main/native/include/wpi/RawFrame.h +++ b/wpiutil/src/main/native/include/wpi/RawFrame.h @@ -11,13 +11,15 @@ #include #include #else -#include +#include // NOLINT #endif #ifdef WPI_RAWFRAME_JNI #include "jni_util.h" #endif +// NOLINT + #ifdef __cplusplus extern "C" { #endif From 50f2fd76d66f279e7d88d5fac42bfe905efe2ab4 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 26 Dec 2023 18:03:51 -0800 Subject: [PATCH 7/9] Formatting --- wpiutil/src/main/native/include/wpi/RawFrame.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/wpiutil/src/main/native/include/wpi/RawFrame.h b/wpiutil/src/main/native/include/wpi/RawFrame.h index 904cc1eaf44..1345ff134e0 100644 --- a/wpiutil/src/main/native/include/wpi/RawFrame.h +++ b/wpiutil/src/main/native/include/wpi/RawFrame.h @@ -11,7 +11,9 @@ #include #include #else + #include // NOLINT + #endif #ifdef WPI_RAWFRAME_JNI From 4c390e4381f0b03ff5ddf1e7eea580cf589c7b48 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 26 Dec 2023 18:14:17 -0800 Subject: [PATCH 8/9] Java formatting --- .../apriltag/AprilTagGenerationTest.java | 10 +++--- .../edu/wpi/first/cscore/CameraServerJNI.java | 3 +- .../edu/wpi/first/cscore/raw/RawSource.java | 6 ++-- .../java/edu/wpi/first/util/RawFrame.java | 32 +++++-------------- .../java/edu/wpi/first/util/WPIUtilJNI.java | 6 ++-- 5 files changed, 23 insertions(+), 34 deletions(-) diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java index 8d7e847f4dc..ddedaaf9439 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java @@ -16,19 +16,19 @@ void test36h11() { assertEquals(PixelFormat.kGray, frame.getPixelFormat()); assertEquals(10, frame.getWidth()); assertEquals(10, frame.getHeight()); - assertEquals(96, frame.getStride()); // default stride in apriltag library + 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 * 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 + assertEquals(0, data.get(96 * 8 + 8)); // inner border + assertEquals(-1, data.get(96 * 9 + 9)); // outer border } } diff --git a/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java b/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java index bed47ceb511..ae075bc83a6 100644 --- a/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java +++ b/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java @@ -257,7 +257,8 @@ public static native void setSourceEnumPropertyChoices( public static native long grabRawSinkFrame(int sink, RawFrame frame, long nativeObj); - public static native long grabRawSinkFrameTimeout(int sink, RawFrame frame, long nativeObj, double timeout); + public static native long grabRawSinkFrameTimeout( + int sink, RawFrame frame, long nativeObj, double timeout); public static native String getSinkError(int sink); diff --git a/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java b/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java index 6a74f4f5ec2..1c8bc237092 100644 --- a/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java +++ b/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java @@ -61,8 +61,10 @@ public void putFrame(RawFrame image) { * @param stride size of each row in bytes * @param pixelFormat pixel format */ - 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()); + 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()); } /** diff --git a/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java b/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java index 658232fdbc9..bad2dcc7616 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java @@ -43,12 +43,7 @@ public void close() { * @param stride The number of bytes in each row of image data * @param pixelFormat The PixelFormat of the frame */ - void setDataJNI( - ByteBuffer data, - int width, - int height, - int stride, - int pixelFormat) { + void setDataJNI(ByteBuffer data, int width, int height, int stride, int pixelFormat) { m_data = data; m_width = width; m_height = height; @@ -64,11 +59,7 @@ void setDataJNI( * @param stride The number of bytes in each row of image data * @param pixelFormat The PixelFormat of the frame */ - void setInfoJNI( - int width, - int height, - int stride, - int pixelFormat) { + void setInfoJNI(int width, int height, int stride, int pixelFormat) { m_width = width; m_height = height; m_stride = stride; @@ -84,12 +75,7 @@ void setInfoJNI( * @param stride The number of bytes in each row of image data * @param pixelFormat The PixelFormat of the frame */ - public void setData( - ByteBuffer data, - int width, - int height, - int stride, - PixelFormat pixelFormat) { + public void setData(ByteBuffer data, int width, int height, int stride, PixelFormat pixelFormat) { if (!data.isDirect()) { throw new UnsupportedOperationException("ByteBuffer must be direct"); } @@ -98,7 +84,8 @@ public void setData( m_height = height; m_stride = stride; m_pixelFormat = pixelFormat; - WPIUtilJNI.setRawFrameData(m_nativeObj, data, data.limit(), width, height, stride, pixelFormat.getValue()); + WPIUtilJNI.setRawFrameData( + m_nativeObj, data, data.limit(), width, height, stride, pixelFormat.getValue()); } /** @@ -109,16 +96,13 @@ public void setData( * @param stride The number of bytes in each row of image data * @param pixelFormat The PixelFormat of the frame */ - public void setInfo( - int width, - int height, - int stride, - PixelFormat pixelFormat) { + public void setInfo(int width, int height, int stride, PixelFormat pixelFormat) { m_width = width; m_height = height; m_stride = stride; m_pixelFormat = pixelFormat; - WPIUtilJNI.setRawFrameInfo(m_nativeObj, m_data.limit(), width, height, stride, pixelFormat.getValue()); + WPIUtilJNI.setRawFrameInfo( + m_nativeObj, m_data.limit(), width, height, stride, pixelFormat.getValue()); } /** diff --git a/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java b/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java index a2fb5cb2011..12a0815e13b 100644 --- a/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java +++ b/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java @@ -87,9 +87,11 @@ public static synchronized void forceLoad() throws IOException { static native long getRawFrameDataPtr(long frame); - static native void setRawFrameData(long frame, ByteBuffer data, int size, int width, int height, int stride, int pixelFormat); + static native void setRawFrameData( + long frame, ByteBuffer data, int size, int width, int height, int stride, int pixelFormat); - static native void setRawFrameInfo(long frame, int size, int width, int height, int stride, int pixelFormat); + static native void setRawFrameInfo( + long frame, int size, int width, int height, int stride, int pixelFormat); /** * Waits for a handle to be signaled. From 07e9ead79ef8bd7682ea91f196bcac67a629ba9d Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 26 Dec 2023 18:17:21 -0800 Subject: [PATCH 9/9] Don't check stride exact value in test --- .../apriltag/AprilTagGenerationTest.java | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java index ddedaaf9439..3f3a9e65d98 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java @@ -16,19 +16,19 @@ void test36h11() { 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()); + int stride = frame.getStride(); + assertEquals(stride * 10, 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 + assertEquals(-1, data.get(stride * 0 + 0)); // outer border is white + assertEquals(0, data.get(stride * 1 + 1)); // inner border is black + assertEquals(-1, data.get(stride * 2 + 2)); + assertEquals(-1, data.get(stride * 3 + 3)); + assertEquals(-1, data.get(stride * 4 + 4)); + assertEquals(0, data.get(stride * 5 + 5)); + assertEquals(0, data.get(stride * 6 + 6)); + assertEquals(-1, data.get(stride * 7 + 7)); + assertEquals(0, data.get(stride * 8 + 8)); // inner border + assertEquals(-1, data.get(stride * 9 + 9)); // outer border } }