diff --git a/build.gradle b/build.gradle index e1ba42be5e..a736357745 100644 --- a/build.gradle +++ b/build.gradle @@ -225,10 +225,10 @@ project(":core") { dependencies { compile group: 'com.google.code.findbugs', name: 'jsr305', version: '3.0.1' - compile group: 'org.bytedeco', name: 'javacv', version: '1.1' - compile group: 'org.bytedeco.javacpp-presets', name: 'opencv', version: '3.0.0-1.1' - compile group: 'org.bytedeco.javacpp-presets', name: 'opencv', version: '3.0.0-1.1', classifier: os - compile group: 'org.bytedeco.javacpp-presets', name: 'opencv-3.0.0-1.1', classifier: 'linux-frc' + compile group: 'org.bytedeco', name: 'javacv', version: '1.3.3' + compile group: 'org.bytedeco.javacpp-presets', name: 'opencv', version: '3.2.0-1.3' + compile group: 'org.bytedeco.javacpp-presets', name: 'opencv', version: '3.2.0-1.3', classifier: os + //compile group: 'org.bytedeco.javacpp-presets', name: 'opencv-3.0.0-1.1', classifier: 'linux-frc' compile group: 'org.bytedeco.javacpp-presets', name: 'videoinput', version: '0.200-1.1', classifier: os compile group: 'org.bytedeco.javacpp-presets', name: 'ffmpeg', version: '0.200-1.1', classifier: os compile group: 'org.python', name: 'jython', version: '2.7.0' @@ -253,6 +253,13 @@ project(":core") { compile group: 'org.ros.rosjava_messages', name: 'grip_msgs', version: '0.0.1' compile group: 'edu.wpi.first.wpilib.networktables.java', name: 'NetworkTables', version: '3.1.2', classifier: 'desktop' compile group: 'edu.wpi.first.wpilib.networktables.java', name: 'NetworkTables', version: '3.1.2', classifier: 'arm' + + // cscore dependencies + compile group: 'edu.wpi.first.cscore', name: 'cscore-java', version: '1.1.0-beta-2' + compile group: 'edu.wpi.first.cscore', name: 'cscore-jni', version: '1.1.0-beta-2', classifier: 'all' + runtime group: 'edu.wpi.first.wpiutil', name: 'wpiutil-java', version: '3.+' + compile group: 'org.opencv', name: 'opencv-java', version: '3.2.0' + compile group: 'org.opencv', name: 'opencv-jni', version: '3.2.0', classifier: 'all' } mainClassName = 'edu.wpi.grip.core.Main' diff --git a/core/src/main/java/edu/wpi/grip/core/operations/composite/PublishVideoOperation.java b/core/src/main/java/edu/wpi/grip/core/operations/composite/PublishVideoOperation.java index 6d6bf53dfb..82e2bb0740 100644 --- a/core/src/main/java/edu/wpi/grip/core/operations/composite/PublishVideoOperation.java +++ b/core/src/main/java/edu/wpi/grip/core/operations/composite/PublishVideoOperation.java @@ -6,35 +6,40 @@ import edu.wpi.grip.core.sockets.InputSocket; import edu.wpi.grip.core.sockets.OutputSocket; import edu.wpi.grip.core.sockets.SocketHints; +import edu.wpi.grip.core.util.OpenCvShims; +import com.google.common.annotations.VisibleForTesting; import com.google.common.collect.ImmutableList; import com.google.inject.Inject; import edu.umd.cs.findbugs.annotations.SuppressFBWarnings; - -import org.bytedeco.javacpp.BytePointer; -import org.bytedeco.javacpp.IntPointer; - -import java.io.DataInputStream; -import java.io.DataOutputStream; -import java.io.IOException; -import java.net.ServerSocket; -import java.net.Socket; +import edu.wpi.cscore.CameraServerJNI; +import edu.wpi.cscore.CvSource; +import edu.wpi.cscore.MjpegServer; +import edu.wpi.cscore.VideoMode; +import edu.wpi.first.wpilibj.networktables.NetworkTable; +import edu.wpi.first.wpilibj.tables.ITable; + +import org.bytedeco.javacpp.opencv_core; +import org.opencv.core.Mat; + +import java.net.Inet4Address; +import java.net.NetworkInterface; +import java.net.SocketException; +import java.util.Collection; +import java.util.Collections; +import java.util.Deque; +import java.util.LinkedList; import java.util.List; import java.util.logging.Level; import java.util.logging.Logger; - -import static org.bytedeco.javacpp.opencv_core.Mat; -import static org.bytedeco.javacpp.opencv_imgcodecs.CV_IMWRITE_JPEG_QUALITY; -import static org.bytedeco.javacpp.opencv_imgcodecs.imencode; +import java.util.stream.Collectors; +import java.util.stream.Stream; /** * Publish an M-JPEG stream with the protocol used by SmartDashboard and the FRC Dashboard. This * allows FRC teams to view video streams on their dashboard during competition even when GRIP has - * exclusive access to the camera. In addition, an intermediate processed image in the pipeline - * could be published instead. Based on WPILib's CameraServer class: - * https://github.com/robotpy/allwpilib/blob/master/wpilibj/src/athena/java/edu/wpi/first/wpilibj - * /CameraServer.java + * exclusive access to the camera. Uses cscore to host the image streaming server. */ @Description(name = "Publish Video", summary = "Publish an MJPEG stream", @@ -43,112 +48,89 @@ public class PublishVideoOperation implements Operation { private static final Logger logger = Logger.getLogger(PublishVideoOperation.class.getName()); - private static final int PORT = 1180; - private static final byte[] MAGIC_NUMBER = {0x01, 0x00, 0x00, 0x00}; - @SuppressWarnings("PMD.AssignmentToNonFinalStatic") - private static int numSteps; - private final Object imageLock = new Object(); - private final BytePointer imagePointer = new BytePointer(); - private final Thread serverThread; - private final InputSocket inputSocket; - private final InputSocket qualitySocket; - @SuppressWarnings("PMD.SingularField") - private volatile boolean connected = false; /** - * Listens for incoming connections on port 1180 and writes JPEG data whenever there's a new - * frame. + * Flags whether or not cscore was loaded. If it could not be loaded, the MJPEG streaming server + * can't be started, preventing this operation from running. */ - private final Runnable runServer = () -> { - // Loop forever (or at least until the thread is interrupted). This lets us recover from the - // dashboard - // disconnecting or the network connection going away temporarily. - while (!Thread.currentThread().isInterrupted()) { - try (ServerSocket serverSocket = new ServerSocket(PORT)) { - logger.info("Starting camera server"); - - try (Socket socket = serverSocket.accept()) { - logger.info("Got connection from " + socket.getInetAddress()); - connected = true; - - DataOutputStream socketOutputStream = new DataOutputStream(socket.getOutputStream()); - DataInputStream socketInputStream = new DataInputStream(socket.getInputStream()); - - byte[] buffer = new byte[128 * 1024]; - int bufferSize; - - final int fps = socketInputStream.readInt(); - final int compression = socketInputStream.readInt(); - final int size = socketInputStream.readInt(); - - if (compression != -1) { - logger.warning("Dashboard video should be in HW mode"); - } - - final long frameDuration = 1000000000L / fps; - long startTime = System.nanoTime(); - - while (!socket.isClosed() && !Thread.currentThread().isInterrupted()) { - // Wait for the main thread to put a new image. This happens whenever perform() is - // called with - // a new input. - synchronized (imageLock) { - imageLock.wait(); - - // Copy the image data into a pre-allocated buffer, growing it if necessary - bufferSize = imagePointer.limit(); - if (bufferSize > buffer.length) { - buffer = new byte[imagePointer.limit()]; - } - imagePointer.get(buffer, 0, bufferSize); - } - - // The FRC dashboard image protocol consists of a magic number, the size of the image - // data, - // and the image data itself. - socketOutputStream.write(MAGIC_NUMBER); - socketOutputStream.writeInt(bufferSize); - socketOutputStream.write(buffer, 0, bufferSize); - - // Limit the FPS to whatever the dashboard requested - int remainingTime = (int) (frameDuration - (System.nanoTime() - startTime)); - if (remainingTime > 0) { - Thread.sleep(remainingTime / 1000000, remainingTime % 1000000); - } - - startTime = System.nanoTime(); - } - } - } catch (IOException e) { - logger.log(Level.WARNING, e.getMessage(), e); - } catch (InterruptedException e) { - Thread.currentThread().interrupt(); // This is really unnecessary since the thread is - // about to exit - logger.info("Shutting down camera server"); - return; - } finally { - connected = false; - } + private static final boolean cscoreLoaded; + + static { + boolean loaded; + try { + // Loading the CameraServerJNI class will load the appropriate platform-specific OpenCV JNI + CameraServerJNI.getHostname(); + loaded = true; + } catch (Throwable e) { + logger.log(Level.SEVERE, "CameraServerJNI load failed!", e); + loaded = false; } - }; + cscoreLoaded = loaded; + } + + private static final int INITIAL_PORT = 1180; + private static final int MAX_STEP_COUNT = 10; // limit ports to 1180-1189 + + @SuppressWarnings("PMD.AssignmentToNonFinalStatic") + private static int totalStepCount; + @SuppressWarnings("PMD.AssignmentToNonFinalStatic") + private static int numSteps; + private static final Deque availablePorts = + Stream.iterate(INITIAL_PORT, i -> i + 1) + .limit(MAX_STEP_COUNT) + .collect(Collectors.toCollection(LinkedList::new)); + + private final InputSocket inputSocket; + private final InputSocket qualitySocket; + private final MjpegServer server; + private final CvSource serverSource; + + // Write to the /CameraPublisher table so the MJPEG streams are discoverable by other + // applications connected to the same NetworkTable server (eg Shuffleboard) + private final ITable cameraPublisherTable = NetworkTable.getTable("/CameraPublisher"); // NOPMD + private final ITable ourTable; + private final Mat publishMat = new Mat(); + private long lastFrame = -1; @Inject @SuppressWarnings("JavadocMethod") @SuppressFBWarnings(value = "ST_WRITE_TO_STATIC_FROM_INSTANCE_METHOD", justification = "Do not need to synchronize inside of a constructor") public PublishVideoOperation(InputSocket.Factory inputSocketFactory) { - if (numSteps != 0) { - throw new IllegalStateException("Only one instance of PublishVideoOperation may exist"); + if (numSteps >= MAX_STEP_COUNT) { + throw new IllegalStateException( + "Only " + MAX_STEP_COUNT + " instances of PublishVideoOperation may exist"); } this.inputSocket = inputSocketFactory.create(SocketHints.Inputs.createMatSocketHint("Image", false)); this.qualitySocket = inputSocketFactory.create(SocketHints.Inputs .createNumberSliderSocketHint("Quality", 80, 0, 100)); - numSteps++; - serverThread = new Thread(runServer, "Camera Server"); - serverThread.setDaemon(true); - serverThread.start(); + if (cscoreLoaded) { + int ourPort = availablePorts.removeFirst(); + + server = new MjpegServer("GRIP video publishing server " + totalStepCount, ourPort); + serverSource = new CvSource("GRIP CvSource " + totalStepCount, + VideoMode.PixelFormat.kMJPEG, 0, 0, 0); + server.setSource(serverSource); + + ourTable = cameraPublisherTable.getSubTable("GRIP-" + totalStepCount); + try { + List networkInterfaces = + Collections.list(NetworkInterface.getNetworkInterfaces()); + ourTable.putStringArray("streams", generateStreams(networkInterfaces, ourPort)); + } catch (SocketException e) { + logger.log(Level.WARNING, "Could not enumerate the local network interfaces", e); + ourTable.putStringArray("streams", new String[0]); + } + } else { + server = null; + serverSource = null; + ourTable = null; + } + + numSteps++; + totalStepCount++; } @Override @@ -166,25 +148,76 @@ public List getOutputSockets() { @Override public void perform() { - if (!connected) { - return; // Don't waste any time converting images if there's no dashboard connected + final long now = System.nanoTime(); // NOPMD + + if (!cscoreLoaded) { + throw new IllegalStateException( + "cscore could not be loaded. The image streaming server cannot be started."); } - if (inputSocket.getValue().get().empty()) { + opencv_core.Mat input = inputSocket.getValue().get(); + if (input.empty() || input.isNull()) { throw new IllegalArgumentException("Input image must not be empty"); } - synchronized (imageLock) { - imencode(".jpeg", inputSocket.getValue().get(), imagePointer, - new IntPointer(CV_IMWRITE_JPEG_QUALITY, qualitySocket.getValue().get().intValue())); - imageLock.notifyAll(); + OpenCvShims.copyJavaCvMatToOpenCvMat(input, publishMat); + // Make sure the output resolution is up to date. Might not be needed, depends on cscore updates + serverSource.setResolution(input.size().width(), input.size().height()); + serverSource.putFrame(publishMat); + if (lastFrame != -1) { + long dt = now - lastFrame; + serverSource.setFPS((int) (1e9 / dt)); } + lastFrame = now; } @Override public synchronized void cleanUp() { - // Stop the video server if there are no Publish Video steps left - serverThread.interrupt(); numSteps--; + if (cscoreLoaded) { + availablePorts.addFirst(server.getPort()); + ourTable.getKeys().forEach(ourTable::delete); + serverSource.setConnected(false); + serverSource.free(); + server.free(); + publishMat.release(); + } + } + + /** + * Generates an array of stream URLs that allow third-party applications to discover the + * appropriate URLs that can stream MJPEG. The URLs will all point to the same physical machine, + * but may use different network interfaces (eg WiFi and ethernet). + * + * @param networkInterfaces the local network interfaces + * @param serverPort the port the mjpeg streaming server is running on + * @return an array of URLs that can be used to connect to the MJPEG streaming server + */ + @VisibleForTesting + static String[] generateStreams(Collection networkInterfaces, int serverPort) { + return networkInterfaces.stream() + .flatMap(i -> Collections.list(i.getInetAddresses()).stream()) + .filter(a -> a instanceof Inet4Address) // IPv6 isn't well supported, stick to IPv4 + .filter(a -> !a.isLoopbackAddress()) // loopback addresses only work for local processes + .distinct() + .flatMap(a -> Stream.of( + generateStreamUrl(a.getHostName(), serverPort), + generateStreamUrl(a.getHostAddress(), serverPort))) + .distinct() + .toArray(String[]::new); + } + + /** + * Generates a URL that can be used to connect to an MJPEG stream provided by cscore. The host + * should be a non-loopback IPv4 address that is resolvable by applications running on non-local + * machines. + * + * @param host the server host + * @param port the port the server is running on + */ + @VisibleForTesting + static String generateStreamUrl(String host, int port) { + return String.format("mjpeg:http://%s:%d/?action=stream", host, port); } + } diff --git a/core/src/main/java/edu/wpi/grip/core/operations/composite/SaveImageOperation.java b/core/src/main/java/edu/wpi/grip/core/operations/composite/SaveImageOperation.java index 5935dc04df..4c6fd545ed 100644 --- a/core/src/main/java/edu/wpi/grip/core/operations/composite/SaveImageOperation.java +++ b/core/src/main/java/edu/wpi/grip/core/operations/composite/SaveImageOperation.java @@ -120,9 +120,9 @@ public void perform() { imencode("." + fileTypesSocket.getValue().get(), inputSocket.getValue().get(), imagePointer, new IntPointer(CV_IMWRITE_JPEG_QUALITY, qualitySocket.getValue().get().intValue())); byte[] buffer = new byte[128 * 1024]; - int bufferSize = imagePointer.limit(); + int bufferSize = (int) imagePointer.limit(); if (bufferSize > buffer.length) { - buffer = new byte[imagePointer.limit()]; + buffer = new byte[bufferSize]; } imagePointer.get(buffer, 0, bufferSize); diff --git a/core/src/main/java/edu/wpi/grip/core/operations/opencv/MinMaxLoc.java b/core/src/main/java/edu/wpi/grip/core/operations/opencv/MinMaxLoc.java index 993b2fc160..4b38999ff5 100644 --- a/core/src/main/java/edu/wpi/grip/core/operations/opencv/MinMaxLoc.java +++ b/core/src/main/java/edu/wpi/grip/core/operations/opencv/MinMaxLoc.java @@ -10,6 +10,7 @@ import com.google.common.collect.ImmutableList; import com.google.inject.Inject; +import org.bytedeco.javacpp.DoublePointer; import org.bytedeco.javacpp.opencv_core; import org.bytedeco.javacpp.opencv_core.Mat; import org.bytedeco.javacpp.opencv_core.Point; @@ -86,14 +87,14 @@ public void perform() { if (mask.empty()) { mask = null; } - final double[] minVal = new double[1]; - final double[] maxVal = new double[1]; + DoublePointer minVal = new DoublePointer(0.0); + DoublePointer maxVal = new DoublePointer(0.0); final Point minLoc = minLocSocket.getValue().get(); final Point maxLoc = maxLocSocket.getValue().get(); opencv_core.minMaxLoc(src, minVal, maxVal, minLoc, maxLoc, mask); - minValSocket.setValue(minVal[0]); - maxValSocket.setValue(maxVal[0]); + minValSocket.setValue(minVal.get(0)); + maxValSocket.setValue(maxVal.get(0)); minLocSocket.setValue(minLocSocket.getValue().get()); maxLocSocket.setValue(maxLocSocket.getValue().get()); } diff --git a/core/src/main/java/edu/wpi/grip/core/util/OpenCvShims.java b/core/src/main/java/edu/wpi/grip/core/util/OpenCvShims.java new file mode 100644 index 0000000000..95b8195e2b --- /dev/null +++ b/core/src/main/java/edu/wpi/grip/core/util/OpenCvShims.java @@ -0,0 +1,37 @@ +package edu.wpi.grip.core.util; + +import org.bytedeco.javacpp.opencv_core; +import org.opencv.core.Mat; + +import java.nio.ByteBuffer; + +/** + * Shims for working with OpenCV and JavaCV wrappers. + */ +public final class OpenCvShims { + + private OpenCvShims() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Copies the data of a JavaCV Mat to an OpenCV mat. + */ + public static void copyJavaCvMatToOpenCvMat(opencv_core.Mat src, Mat dst) { + final int width = src.cols(); + final int height = src.rows(); + final int channels = src.channels(); + + final ByteBuffer buffer = src.createBuffer(); + byte[] bytes = new byte[width * height * channels]; + buffer.get(bytes); + + // Store the data in a temporary mat to get the type, size, etc. correct, since Mat doesn't + // expose methods for directly changing its size or type + final Mat tmp = new Mat(src.rows(), src.cols(), src.type()); + tmp.put(0, 0, bytes); + tmp.copyTo(dst); + tmp.release(); + } + +} diff --git a/core/src/test/java/edu/wpi/grip/core/operations/composite/PublishVideoOperationTest.java b/core/src/test/java/edu/wpi/grip/core/operations/composite/PublishVideoOperationTest.java new file mode 100644 index 0000000000..283f5a749c --- /dev/null +++ b/core/src/test/java/edu/wpi/grip/core/operations/composite/PublishVideoOperationTest.java @@ -0,0 +1,92 @@ +package edu.wpi.grip.core.operations.composite; + +import org.junit.Test; + +import java.lang.reflect.Constructor; +import java.net.Inet4Address; +import java.net.InetAddress; +import java.net.NetworkInterface; +import java.util.Arrays; +import java.util.List; + +import static edu.wpi.grip.core.operations.composite.PublishVideoOperation.generateStreamUrl; +import static org.junit.Assert.assertEquals; + +public class PublishVideoOperationTest { + + @Test + public void testGenerateStreams() { + // given + final String firstHost = "localhost"; + final int firstAddress = 0x7F_00_00_01; // loopback 127.0.0.1 + final String secondHost = "driver-station"; + final int secondAddress = 0x0A_01_5A_05; // 10.1.90.5, FRC driver station IP + final String thirdHost = "network-mask"; + final int thirdAddress = 0xFF_FF_FF_FF; // 255.255.255.255, not loopback + final List networkInterfaces = + Arrays.asList( + newNetworkInterface( + "MockNetworkInterface0", 0, + new InetAddress[]{ + newInet4Address(firstHost, firstAddress) + }), + newNetworkInterface("MockNetworkInterface1", 1, + new InetAddress[]{ + newInet4Address(secondHost, secondAddress), + newInet4Address(thirdHost, thirdAddress) + }) + ); + final int port = 54321; + + // when + final String[] streams = PublishVideoOperation.generateStreams(networkInterfaces, port); + + // then + assertEquals("Four URLs should have been generated", 4, streams.length); + + // stream URLs should be generated only for non-loopback IPv4 addresses + assertEquals(generateStreamUrl(secondHost, port), streams[0]); + assertEquals(generateStreamUrl(formatIpv4Address(secondAddress), port), streams[1]); + assertEquals(generateStreamUrl(thirdHost, port), streams[2]); + assertEquals(generateStreamUrl(formatIpv4Address(thirdAddress), port), streams[3]); + + } + + private static String formatIpv4Address(int address) { + return String.format( + "%d.%d.%d.%d", + address >> 24 & 0xFF, + address >> 16 & 0xFF, + address >> 8 & 0xFF, + address & 0xFF + ); + } + + private static NetworkInterface newNetworkInterface(String name, + int index, + InetAddress[] addresses) { + try { + Constructor constructor = + NetworkInterface.class.getDeclaredConstructor( + String.class, + int.class, + InetAddress[].class); + constructor.setAccessible(true); + return constructor.newInstance(name, index, addresses); + } catch (ReflectiveOperationException e) { + throw new AssertionError(e); + } + } + + private static Inet4Address newInet4Address(String hostname, int address) { + try { + Constructor constructor = + Inet4Address.class.getDeclaredConstructor(String.class, int.class); + constructor.setAccessible(true); + return constructor.newInstance(hostname, address); + } catch (ReflectiveOperationException e) { + throw new AssertionError(e); + } + } + +} diff --git a/core/src/test/java/edu/wpi/grip/core/sources/CameraSourceTest.java b/core/src/test/java/edu/wpi/grip/core/sources/CameraSourceTest.java index 29447211f4..2eaac028cc 100644 --- a/core/src/test/java/edu/wpi/grip/core/sources/CameraSourceTest.java +++ b/core/src/test/java/edu/wpi/grip/core/sources/CameraSourceTest.java @@ -281,7 +281,7 @@ static class MockFrameGrabber extends FrameGrabber { for (int y = 0; y < frameIdx.rows(); y++) { for (int x = 0; x < frameIdx.cols(); x++) { for (int z = 0; z < frameIdx.channels(); z++) { - frameIdx.putDouble(new int[]{y, x, z}, y + x + z); + frameIdx.putDouble(new long[]{y, x, z}, y + x + z); } } } diff --git a/core/src/test/java/edu/wpi/grip/core/util/OpenCvShimsTest.java b/core/src/test/java/edu/wpi/grip/core/util/OpenCvShimsTest.java new file mode 100644 index 0000000000..5f09d58c40 --- /dev/null +++ b/core/src/test/java/edu/wpi/grip/core/util/OpenCvShimsTest.java @@ -0,0 +1,55 @@ +package edu.wpi.grip.core.util; + +import edu.wpi.grip.util.Files; + +import org.bytedeco.javacpp.opencv_core; +import org.junit.Test; +import org.opencv.core.CvType; +import org.opencv.core.Mat; + +import java.nio.ByteBuffer; + +import static org.hamcrest.Matchers.greaterThanOrEqualTo; +import static org.junit.Assert.assertArrayEquals; +import static org.junit.Assert.assertEquals; +import static org.junit.Assume.assumeThat; + +public class OpenCvShimsTest { + + @Test + public void testCopyJavaCvToOpenCv() { + // given + final opencv_core.Mat javaCvMat = Files.imageFile.createMat(); + final Mat openCvMat = new Mat(1, 1, CvType.CV_8SC1); + + // when + OpenCvShims.copyJavaCvMatToOpenCvMat(javaCvMat, openCvMat); + + // then + + // test the basic properties (same size, type, etc.) + assertEquals("Wrong width", javaCvMat.cols(), openCvMat.cols()); + assertEquals("Wrong height", javaCvMat.rows(), openCvMat.rows()); + assertEquals("Wrong type", javaCvMat.type(), openCvMat.type()); + assertEquals("Wrong channel amount", javaCvMat.channels(), openCvMat.channels()); + assertEquals("Wrong bit depth", javaCvMat.depth(), openCvMat.depth()); + + // test the raw data bytes - they should be identical + final int width = javaCvMat.cols(); + final int height = javaCvMat.rows(); + final int channels = javaCvMat.channels(); + + final ByteBuffer buffer = javaCvMat.createBuffer(); + assumeThat("JavaCV byte buffer is smaller than expected!", + buffer.capacity(), greaterThanOrEqualTo(width * height * channels)); + + final byte[] javaCvData = new byte[width * height * channels]; + buffer.get(javaCvData); + + final byte[] openCvData = new byte[width * height * channels]; + openCvMat.get(0, 0, openCvData); + + assertArrayEquals("Wrong data bytes", javaCvData, openCvData); + } + +} diff --git a/ui/src/test/java/edu/wpi/grip/ui/codegeneration/tools/HelperTools.java b/ui/src/test/java/edu/wpi/grip/ui/codegeneration/tools/HelperTools.java index d28209e13c..8568fefc15 100644 --- a/ui/src/test/java/edu/wpi/grip/ui/codegeneration/tools/HelperTools.java +++ b/ui/src/test/java/edu/wpi/grip/ui/codegeneration/tools/HelperTools.java @@ -68,7 +68,8 @@ public static double matAvgDiff(Mat mat1, Mat mat2) { */ public static Mat bytedecoMatToCVMat(org.bytedeco.javacpp.opencv_core.Mat input) { UByteIndexer idxer = input.createIndexer(); - Mat out = new Mat(idxer.rows(), idxer.cols(), CvType.CV_8UC(idxer.channels())); + Mat out = new Mat( + (int) idxer.rows(), (int) idxer.cols(), CvType.CV_8UC((int) idxer.channels())); //Mat out = new Mat(idxer.rows(),idxer.cols(),input.type()); for (int row = 0; row < idxer.rows(); row++) { for (int col = 0; col < idxer.cols(); col++) {