Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpiutil,cscore,apriltag] Fix RawFrame (#6098)

Browse Source
This commit is contained in:
Peter Johnson
2023-12-26 20:05:02 -08:00
committed by GitHub
parent 8aeee03626
commit 5659038443
18 changed files with 537 additions and 317 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java
View File

@@ -54,7 +54,9 @@ public class AprilTag {
* @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 class AprilTag {
* @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;
}
}

4
apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java
View File

@@ -191,7 +191,7 @@ public class AprilTagJNI {
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);
}

53
apriltag/src/main/native/cpp/AprilTag.cpp
View File

@@ -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);
tag36h11_destroy(tagFamily);
return markerFrame;
return rv;
}
wpi::RawFrame AprilTag::Generate16h5AprilTagImage(int id) {
bool AprilTag::Generate36h11AprilTagImage(wpi::RawFrame* frame, int id) {
apriltag_family_t* tagFamily = tag36h11_create();
bool rv = FamilyToImage(frame, tagFamily, id);
tag36h11_destroy(tagFamily);
return rv;
}
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) {

47
apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp
View File

@@ -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"

4
apriltag/src/main/native/include/frc/apriltag/AprilTag.h
View File

@@ -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

34
apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java Normal file
View File

@@ -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());
int stride = frame.getStride();
assertEquals(stride * 10, frame.getSize());
// check the diagonal values
var data = frame.getData();
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
}
}

61
cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java
View File

@@ -182,21 +182,13 @@ public class CameraServerJNI {
//
// 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);
int source, ByteBuffer data, int size, int width, int height, int stride, int pixelFormat);
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());
}
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,10 @@ public class CameraServerJNI {
//
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);
public static native long grabRawSinkFrame(int sink, RawFrame frame, long nativeObj);
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 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);

10
cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java
View File

@@ -33,7 +33,7 @@ public class RawSink extends ImageSink {
* @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 @@ public class RawSink extends ImageSink {
* @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 @@ public class RawSink extends ImageSink {
* @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());
}
}

47
cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java
View File

@@ -9,6 +9,7 @@ import edu.wpi.first.cscore.ImageSource;
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,41 @@ public class RawSource extends ImageSource {
*
* @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);
}
/**
* Put a raw image and notify sinks.
*
* @param data raw frame data pointer
* @param width frame width
* @param height frame height
* @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);
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 native ByteBuffer
* @param width frame width
* @param height frame height
* @param stride size of each row in bytes
* @param pixelFormat pixel format
*/
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());
}
}

17
cscore/src/main/native/cpp/Image.h
View File

@@ -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_) {

6
cscore/src/main/native/cpp/RawSinkImpl.cpp
View File

@@ -109,10 +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.data);
rawFrame.size = newImage->size();
std::copy(newImage->data(), newImage->data() + rawFrame.size, rawFrame.data);
return incomingFrame.GetTime();
}

5
cscore/src/main/native/cpp/RawSourceImpl.cpp
View File

@@ -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<size_t>(image.stride)};
std::unique_ptr<Image> dest =
AllocImage(static_cast<VideoMode::PixelFormat>(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());

150
cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
View File

@@ -7,6 +7,9 @@
#include <fmt/format.h>
#include <opencv2/core/core.hpp>
#define WPI_RAWFRAME_JNI
#include <wpi/RawFrame.h>
#include <wpi/SmallString.h>
#include <wpi/jni_util.h>
@@ -1220,48 +1223,78 @@ Java_edu_wpi_first_cscore_CameraServerCvJNI_putSourceFrame
}
}
// int width, int height, int pixelFormat, int totalData
/*
* Class: edu_wpi_first_cscore_CameraServerJNI
* Method: putRawSourceFrameBB
* Signature: (ILjava/lang/Object;IIII)V
* Method: putRawSourceFrame
* Signature: (IJ)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)
Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrame
(JNIEnv* env, jclass, jint source, jlong framePtr)
{
WPI_RawFrame rawFrame;
rawFrame.data =
reinterpret_cast<char*>(env->GetDirectBufferAddress(byteBuffer));
rawFrame.totalData = totalData;
rawFrame.pixelFormat = pixelFormat;
rawFrame.width = width;
rawFrame.height = height;
auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
if (!frame) {
nullPointerEx.Throw(env, "frame is null");
return;
}
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: putRawSourceFrameBB
* Signature: (ILjava/lang/Object;IIIII)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)
Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrameBB
(JNIEnv* env, jclass, jint source, jobject data, jint size, jint width,
jint height, jint stride, jint pixelFormat)
{
WPI_RawFrame rawFrame;
rawFrame.data = reinterpret_cast<char*>(static_cast<intptr_t>(ptr));
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<uint8_t*>(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: putRawSourceFrameData
* Signature: (IJIIIII)V
*/
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)
{
WPI_RawFrame frame; // use WPI_Frame because we don't want the destructor
frame.data = reinterpret_cast<uint8_t*>(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, frame, &status);
CheckStatus(env, status);
}
@@ -1722,72 +1755,47 @@ 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<jlong>(reinterpret_cast<intptr_t>(frame.data));
if (didChangeDataPtr) {
byteBuffer = env->NewDirectByteBuffer(frame.data, frame.dataLength);
}
env->CallVoidMethod(
rawFrameObj, setMethod, byteBuffer, framePtr,
static_cast<jint>(frame.totalData), static_cast<jint>(frame.width),
static_cast<jint>(frame.height), static_cast<jint>(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)
Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrame
(JNIEnv* env, jclass, jint sink, jobject frameObj, jlong framePtr)
{
WPI_RawFrame* ptr =
reinterpret_cast<WPI_RawFrame*>(static_cast<intptr_t>(rawFramePtr));
auto origDataPtr = ptr->data;
ptr->width = width;
ptr->height = height;
ptr->pixelFormat = pixelFormat;
auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
auto origData = frame->data;
CS_Status status = 0;
auto rv = cs::GrabSinkFrame(static_cast<CS_Sink>(sink), *ptr, &status);
auto rv = cs::GrabSinkFrame(static_cast<CS_Sink>(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<WPI_RawFrame*>(static_cast<intptr_t>(rawFramePtr));
auto origDataPtr = ptr->data;
ptr->width = width;
ptr->height = height;
ptr->pixelFormat = pixelFormat;
auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
auto origData = frame->data;
CS_Status status = 0;
auto rv = cs::GrabSinkFrameTimeout(static_cast<CS_Sink>(sink), *ptr, timeout,
&status);
auto rv = cs::GrabSinkFrameTimeout(static_cast<CS_Sink>(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;
}

150
wpiutil/src/main/java/edu/wpi/first/util/RawFrame.java
View File

@@ -12,17 +12,16 @@ import java.nio.ByteBuffer;
* <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 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,79 @@ public class RawFrame implements AutoCloseable {
*/
@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,
int width,
int height,
int pixelFormat) {
m_dataByteBuffer = dataByteBuffer;
m_dataPtr = dataPtr;
m_totalData = 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_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 +110,8 @@ public class RawFrame implements AutoCloseable {
*
* @return The pointer to native representation of this frame.
*/
public long getFramePtr() {
return m_framePtr;
public long getNativeObj() {
return m_nativeObj;
}
/**
@@ -75,64 +121,55 @@ public class RawFrame implements AutoCloseable {
*
* @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 +177,7 @@ public class RawFrame implements AutoCloseable {
*
* @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;
}
}

13
wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java
View File

@@ -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,17 @@ public class WPIUtilJNI {
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.

43
wpiutil/src/main/native/cpp/RawFrame.cpp
View File

@@ -6,25 +6,44 @@
#include <wpi/MemAlloc.h>
#include <cstring>
extern "C" {
void WPI_AllocateRawFrameData(WPI_RawFrame* frame, int requestedSize) {
if (frame->dataLength >= requestedSize) {
return;
int WPI_AllocateRawFrameData(WPI_RawFrame* frame, size_t requestedSize) {
if (frame->capacity >= requestedSize) {
return 0;
}
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;
WPI_FreeRawFrameData(frame);
frame->data = static_cast<uint8_t*>(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<uint8_t*>(data);
frame->freeFunc = freeFunc;
frame->freeCbData = cbdata;
frame->capacity = capacity;
frame->size = size;
}
} // extern "C"

77
wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
View File

@@ -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<intptr_t>(rawFrame);
return static_cast<jlong>(rawFrameIntPtr);
return reinterpret_cast<jlong>(new wpi::RawFrame);
}
/*
@@ -339,11 +337,76 @@ 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<wpi::RawFrame*>(static_cast<intptr_t>(rawFrame));
delete ptr;
delete reinterpret_cast<wpi::RawFrame*>(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<wpi::RawFrame*>(frame);
if (!f) {
wpi::ThrowNullPointerException(env, "frame is null");
return 0;
}
return reinterpret_cast<jlong>(f->data);
}
/*
* Class: edu_wpi_first_util_WPIUtilJNI
* Method: setRawFrameData
* 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)
{
auto* f = reinterpret_cast<wpi::RawFrame*>(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<wpi::RawFrame*>(frame);
if (!f) {
wpi::ThrowNullPointerException(env, "frame is null");
return;
}
f->width = width;
f->height = height;
f->stride = stride;
f->pixelFormat = pixelFormat;
}
} // extern "C"

125
wpiutil/src/main/native/include/wpi/RawFrame.h
View File

@@ -5,38 +5,65 @@
#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;
#include <stdint.h>
#ifdef __cplusplus
#include <concepts>
#include <cstddef>
#else
#include <stddef.h> // NOLINT
#endif
#ifdef WPI_RAWFRAME_JNI
#include "jni_util.h"
#endif
// NOLINT
#ifdef __cplusplus
extern "C" {
#endif
/**
* Raw Frame
*/
typedef struct WPI_RawFrame { // NOLINT
// 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;
/**
* 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 +74,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<WPI_RawFrame*>(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 <std::same_as<wpi::RawFrame> 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<jint>(frame.width), static_cast<jint>(frame.height),
static_cast<jint>(frame.stride), static_cast<jint>(frame.pixelFormat));
} else {
static jmethodID setInfo =
env->GetMethodID(rawFrameCls, "setInfoJNI", "(IIII)V");
env->CallVoidMethod(jframe, setInfo, static_cast<jint>(frame.width),
static_cast<jint>(frame.height),
static_cast<jint>(frame.stride),
static_cast<jint>(frame.pixelFormat));
}
}
#endif
} // namespace wpi
#endif
#endif // WPIUTIL_WPI_RAWFRAME_H_