|
1
|
|
|
package org.usfirst.frc.team3695.robot; |
|
2
|
|
|
|
|
3
|
|
|
import edu.wpi.cscore.CvSink; |
|
4
|
|
|
import edu.wpi.cscore.CvSource; |
|
5
|
|
|
import edu.wpi.cscore.UsbCamera; |
|
6
|
|
|
|
|
7
|
|
|
import edu.wpi.first.wpilibj.CameraServer; |
|
8
|
|
|
import edu.wpi.first.wpilibj.IterativeRobot; |
|
9
|
|
|
|
|
10
|
|
|
import org.opencv.core.Core; |
|
11
|
|
|
import org.opencv.core.Mat; |
|
12
|
|
|
|
|
13
|
|
|
import java.util.ArrayList; |
|
14
|
|
|
|
|
15
|
|
|
/** |
|
16
|
|
|
* Contains methods used for anything vision |
|
17
|
|
|
*/ |
|
18
|
|
|
public class Vision extends IterativeRobot { |
|
19
|
|
|
|
|
20
|
|
|
/// Two cameras for double FOV |
|
21
|
|
|
private UsbCamera cameraLeft; |
|
22
|
|
|
private UsbCamera cameraRight; |
|
23
|
|
|
|
|
24
|
|
|
/** |
|
25
|
|
|
* Initialization of class |
|
26
|
|
|
*/ |
|
27
|
|
|
public void robotInit() { |
|
28
|
|
|
// Places the vision in a separate thread from everything else as recommended by FIRST |
|
29
|
|
|
// It should never be accessed by other code, so protection isn't necessary. |
|
30
|
|
|
new Thread(this::startCamerasStream).start(); |
|
31
|
|
|
} |
|
32
|
|
|
|
|
33
|
|
|
/** |
|
34
|
|
|
* Start both the left and right camera streams and combine them into a single one which is then pushed |
|
35
|
|
|
* to an output stream titled Concat |
|
36
|
|
|
*/ |
|
37
|
|
|
private void startCamerasStream() { |
|
38
|
|
|
cameraLeft = CameraServer.getInstance().startAutomaticCapture("Left", 0); |
|
39
|
|
|
cameraRight = CameraServer.getInstance().startAutomaticCapture("Right", 1); |
|
40
|
|
|
|
|
41
|
|
|
/// Dummy sinks to keep camera connections open. |
|
42
|
|
|
CvSink cvsinkLeft = new CvSink("leftSink"); |
|
43
|
|
|
cvsinkLeft.setSource(cameraLeft); |
|
44
|
|
|
cvsinkLeft.setEnabled(true); |
|
45
|
|
|
CvSink cvsinkRight = new CvSink("rightSink"); |
|
46
|
|
|
cvsinkRight.setSource(cameraRight); |
|
47
|
|
|
cvsinkRight.setEnabled(true); |
|
48
|
|
|
|
|
49
|
|
|
/// Matrices to store each image from the cameras. |
|
50
|
|
|
Mat leftSource = new Mat(); |
|
51
|
|
|
Mat rightSource = new Mat(); |
|
52
|
|
|
|
|
53
|
|
|
/// The ArrayList of left and right sources is needed for the hconcat method used to combine the streams |
|
54
|
|
|
ArrayList<Mat> sources = new ArrayList<>(); |
|
55
|
|
|
sources.add(leftSource); |
|
56
|
|
|
sources.add(rightSource); |
|
57
|
|
|
|
|
58
|
|
|
/// Concatenation of both matrices |
|
59
|
|
|
Mat concat = new Mat(); |
|
60
|
|
|
|
|
61
|
|
|
/// Puts the combined video on the SmartDashboard (I think) |
|
62
|
|
|
/// The width is multiplied by 2 as the dimensions of the stream will have a width two times that of a single webcam |
|
63
|
|
|
CvSource outputStream = CameraServer.getInstance().putVideo("Concat", 2*Constants.CAM_WIDTH, Constants.CAM_HEIGHT); |
|
|
|
|
|
|
64
|
|
|
|
|
65
|
|
|
while (!Thread.interrupted()) { |
|
66
|
|
|
/// Provide each mat with the current frame |
|
67
|
|
|
cvsinkLeft.grabFrame(leftSource); |
|
68
|
|
|
cvsinkRight.grabFrame(rightSource); |
|
69
|
|
|
/// Combine the frames into a single mat in the Output and stream the image. |
|
70
|
|
|
Core.hconcat(sources, concat); |
|
71
|
|
|
outputStream.putFrame(concat); |
|
72
|
|
|
} |
|
73
|
|
|
} |
|
74
|
|
|
} |
|
75
|
|
|
|
wh1ter0se /
PowerUp-2018
Using constants for hard-coded numbers is a best practice. A constant’s name can explain the rationale behind this magic number. It is also easier to find if you ever need to change it.