Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Use cscore for PublishVideoOperation #866

Open
wants to merge 11 commits into
base: master
Choose a base branch
from
15 changes: 11 additions & 4 deletions build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -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'
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Commented out because there's no opencv-3.2.0-1.3 with the linux-frc classifier and I'm worried about different memory models between binding versions (JavaCV with 3.0.0, OpenCV with 3.2.0) . This could get removed entirely if we remove the deploy functionality.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

With this commented out as it is is the deploy functionality essentially nerfed? If so, then we should remove the deployer for this season.

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'
Expand All @@ -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'
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -10,34 +10,47 @@
import com.google.common.collect.ImmutableList;

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.lang.reflect.Field;
import java.util.Deque;
import java.util.LinkedList;
import java.util.List;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.stream.Collectors;
import java.util.stream.Stream;

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 static org.bytedeco.javacpp.opencv_core.CV_8S;
import static org.bytedeco.javacpp.opencv_core.CV_8U;

/**
* 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.
*/
public class PublishVideoOperation implements Operation {

private static final Logger logger = Logger.getLogger(PublishVideoOperation.class.getName());

static {
try {
// Loading the CameraServerJNI class will load the appropriate platform-specific OpenCV JNI
CameraServerJNI.getHostname();
} catch (Throwable e) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can native bindings ever throw an InterruptedException? If so, make sure that this handles it correctly.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's most likely to be UnsatisfiedLinkError or a related error. No interrupted exceptions here.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Unless we explicitly throw interrupted from the JNI level, nothing in JNI otherwise would throw it. And since its a dumb idea to interrupt a thread anyway, we would never throw it internally. Pretty much all that shows up is UnsatisfiedLinkError, or one of the few runtime exceptions we throw.

Note this should be using forceLoad(), not getHostname() (I don't think we had forceLoad() then), but I will my sure my version does that.

logger.log(Level.SEVERE, "CameraServerJNI load failed! Exiting", e);
System.exit(31);
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This should set a flag and make the operation throw an exception in perform() instead of suddenly exiting the entire application

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yea, we want to limit the number crashes that don't display the crash dialog.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Theres actually nothing to catch, because internally in the JNI loading classes we call System.exit(1). There would be no exception ever caught here. It would be impossible to catch.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

After a little discussion, we will leave the IO Exception that the static constructor can throw in flight, that way its catchable. That will be a change on our side. We will do the same thing to ntcore as well.

}
}
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does this need to happen statically? Can't this be done when the operation is constructed?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Then the cscore loading check would happen every time a new operation is created instead of only once, when the class loads. I'd prefer to just have a flag set so the operation can be created normally and just throw an exception in perform() so users can see the operation failing (and hopefully report it to us)


public static final OperationDescription DESCRIPTION =
OperationDescription.builder()
.name("Publish Video")
Expand All @@ -46,110 +59,55 @@ public class PublishVideoOperation implements Operation {
.icon(Icon.iconStream("publish-video"))
.build();
private static final int PORT = 1180;
private static final byte[] MAGIC_NUMBER = {0x01, 0x00, 0x00, 0x00};

@SuppressWarnings("PMD.AssignmentToNonFinalStatic")
private static int totalStepCount;
@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<Mat> inputSocket;
private static final int MAX_STEP_COUNT = 10; // limit ports to 1180-1189
private static final Deque<Integer> availablePorts =
Stream.iterate(PORT, i -> i + 1)
.limit(MAX_STEP_COUNT)
.collect(Collectors.toCollection(LinkedList::new));

private final InputSocket<opencv_core.Mat> inputSocket;
private final InputSocket<Number> 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.
*/
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 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 static final ITable cameraPublisherTable = NetworkTable.getTable("/CameraPublisher");
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you not load this statically? Instead do it at instantiation time.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What would that gain?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If it fails then GRIP doesn't crash immediately.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So instead of crashing to desktop at initialization when creating the operations list, it'll crash to desktop when adding a new PublishVideo operation. I guess that's better because at least users will see it happens when adding the operation

Copy link
Member Author

@SamCarlberg SamCarlberg Nov 9, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We'll also need to update NetworkTables to 4.0, but that's a different PR

private final ITable ourTable;
private final Mat publishMat = new Mat();
private long lastFrame = -1;

@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();
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);
ourTable.putStringArray("streams",
new String[]{CameraServerJNI.getHostname() + ":" + ourPort + "/?action=stream"});

numSteps++;
totalStepCount++;
}

@Override
Expand All @@ -167,25 +125,51 @@ public List<OutputSocket> getOutputSockets() {

@Override
public void perform() {
if (!connected) {
return; // Don't waste any time converting images if there's no dashboard connected
}

if (inputSocket.getValue().get().empty()) {
final long now = System.nanoTime(); // NOPMD
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();
copyJavaCvToOpenCvMat(input, publishMat);
serverSource.putFrame(publishMat);
if (lastFrame != -1) {
long dt = now - lastFrame;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've seen this actually be 0 even though you are using nano seconds. (This is highly unlikely but we saw it in the camera source code in one bug) This will cause a DIV by zero exception.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It'll actually be Double.Infinity per IEEE spec for floating-point numbers (1e9 / 0L = Infinity)

Copy link
Member

@JLLeitschuh JLLeitschuh Nov 9, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(int) 1e9 / 0L will throw java.lang.ArithmeticException: / by zero because of the cast.
Cast in the wrong place.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In kotlin: (1e9 / 0L).toInt() is equal to 2147483647 which is an insanely high frame rate. Is that what you want to set it to?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's technically infinite since the new frame comes at the same time as the previous one. But it doesn't happen here because it's rate limited by the pipeline and the update rate of the pipeline sources. I seriously doubt dt will ever be less than a few hundred microseconds.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay, that's fine then.

serverSource.setFPS((int) (1e9 / dt));
}
lastFrame = now;
server.setSource(serverSource);
}

@Override
public synchronized void cleanUp() {
// Stop the video server if there are no Publish Video steps left
serverThread.interrupt();
numSteps--;
availablePorts.addFirst(server.getPort());
ourTable.getKeys().forEach(ourTable::delete);
}

private void copyJavaCvToOpenCvMat(opencv_core.Mat javaCvMat, Mat openCvMat) {
if (javaCvMat.depth() != CV_8U && javaCvMat.depth() != CV_8S) {
throw new IllegalArgumentException("Only 8-bit depth images are supported");
}

final opencv_core.Size size = javaCvMat.size();

// Make sure the output resolution is up to date
serverSource.setResolution(size.width(), size.height());

// Make the OpenCV Mat object point to the same block of memory as the JavaCV object.
// This requires no data transfers or copies and is O(1) instead of O(n)
if (javaCvMat.address() != openCvMat.nativeObj) {
try {
Field nativeObjField = Mat.class.getField("nativeObj");
nativeObjField.setAccessible(true);
nativeObjField.setLong(openCvMat, javaCvMat.address());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This seems like a terrible idea, but I understand what you are doing here.
Do you actually see speed benefits from doing this?
@saudet Is there any reason why doing this assignment could shoot us in the foot?

This is taking the memory address of the OpenCV mat and basically telling JavaCV to use it's memory instead.

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

First it'd say, what is missing from JavaCV that you need to use the other set of bindings? IMO, we should fix that so we don't need to do things like that.

And even when that's necessary, we don't need to use reflection.

Just passing the pointer address around is sufficient, as long as you make sure they are from the same version of OpenCV, compiled with the same compiler, the same flags, etc and to make sure memory isn't prematurely deallocated and that it gets deallocated by the same binaries used to allocate it.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there a way of creating a Mat just using the memory address??

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@SamCarlberg Ping on this? Thoughts on making sure the memory isn't inconsistently GCed?

Copy link
Contributor

@ThadHouse ThadHouse Apr 19, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

See my comment at the bottom. In the rewrite, all the memory mapping between different opencv versions will be gone. There will be no wpilib opencv interface anymore. And the API designed is allocation free.

Copy link
Contributor

@ThadHouse ThadHouse Apr 19, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As an example, The following class would be added to GRIP. It will use the JavaCV version of OpenCV, and always copy from the raw image into the requested Mat. It is memory safe, and does not depend at all on the internal representation of how an OpenCV Mat looks. There's some tricks on the cscore jni side to avoid allocations when needed, but everything is in spec, no reflection or undefined behavior.

https://github.com/wpilibsuite/allwpilib/blob/13e15663b6570028a9222b4990d141d16fda3a72/cscore/src/dev/java/edu/wpi/cscore/RawCVMatSink.java

This would be what the source side looks like
https://github.com/wpilibsuite/allwpilib/blob/d56dee9c493e8de59340b22d4cc43bc8ba40239d/cscore/src/dev/java/edu/wpi/cscore/RawCVMatSource.java

We internally copy the pointer, so again not depending on any memory issues, or native layouts.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@SamCarlberg Do we want to wait for that update then?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think so. Thad's offered to do do the rewrite with the new cscore stuff. Maybe we should also use the cameraserver library for doing the stream hosting instead of manually doing the Networktable side

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I wouldn't be able to bring it in directly (because of the opencv dependency) but I could bring in most of it. There's a lot extra we wouldnt need there as well.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I wouldn't be able to bring it in directly (because of the opencv dependency) but I could bring in most of it. There's a lot extra we wouldnt need there as well.

} catch (ReflectiveOperationException e) {
logger.log(Level.WARNING, "Could not set native object pointer", e);
}
}
}

}
Original file line number Diff line number Diff line change
Expand Up @@ -124,9 +124,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);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@

import com.google.common.collect.ImmutableList;

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;
Expand Down Expand Up @@ -85,14 +86,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());
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
}
Expand Down