package frc.robot;

import java.io.File;

import java.io.FileWriter;

import java.io.IOException;

import java.util.ArrayList;

import java.util.List;

import java.util.Map;

import java.util.stream.Collectors;

import java.util.HashMap;

import edu.wpi.first.wpilibj.vision.VisionPipeline;

import org.opencv.core.*;

import org.opencv.core.Core.*;

import org.opencv.features2d.FeatureDetector;

import org.opencv.imgcodecs.Imgcodecs;

import org.opencv.imgproc.*;

import org.opencv.objdetect.*;

/**

*/

public class GripPipeline implements VisionPipeline {

```
//Outputs
private Mat cvResizeOutput = new Mat();
private Mat hsvThresholdOutput = new Mat();
private Mat cvErodeOutput = new Mat();
private Mat maskOutput = new Mat();
private ArrayList<Line> findLinesOutput = new ArrayList<Line>();
static {
System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
}
/**
* This is the primary method that runs the entire pipeline and updates the outputs.
*/
@Override public void process(Mat source0) {
// Step CV_resize0:
Mat cvResizeSrc = source0;
Size cvResizeDsize = new Size(0, 0);
double cvResizeFx = 0.25;
double cvResizeFy = 0.25;
int cvResizeInterpolation = Imgproc.INTER_LINEAR;
cvResize(cvResizeSrc, cvResizeDsize, cvResizeFx, cvResizeFy, cvResizeInterpolation, cvResizeOutput);
// Step HSV_Threshold0:
Mat hsvThresholdInput = cvResizeOutput;
double[] hsvThresholdHue = {0.0, 32.55972696245734};
double[] hsvThresholdSaturation = {43.57014388489208, 109.22354948805462};
double[] hsvThresholdValue = {169.69424460431654, 239.76962457337885};
hsvThreshold(hsvThresholdInput, hsvThresholdHue, hsvThresholdSaturation, hsvThresholdValue, hsvThresholdOutput);
// Step CV_erode0:
Mat cvErodeSrc = hsvThresholdOutput;
Mat cvErodeKernel = new Mat();
Point cvErodeAnchor = new Point(-1, -1);
double cvErodeIterations = 1.0;
int cvErodeBordertype = Core.BORDER_CONSTANT;
Scalar cvErodeBordervalue = new Scalar(-1);
cvErode(cvErodeSrc, cvErodeKernel, cvErodeAnchor, cvErodeIterations, cvErodeBordertype, cvErodeBordervalue, cvErodeOutput);
// Step Mask0:
Mat maskInput = cvResizeOutput;
Mat maskMask = cvErodeOutput;
mask(maskInput, maskMask, maskOutput);
// Step Find_Lines0:
Mat findLinesInput = maskOutput;
findLines(findLinesInput, findLinesOutput);
}
/**
* This method is a generated getter for the output of a CV_resize.
* @return Mat output from CV_resize.
*/
public Mat cvResizeOutput() {
return cvResizeOutput;
}
/**
* This method is a generated getter for the output of a HSV_Threshold.
* @return Mat output from HSV_Threshold.
*/
public Mat hsvThresholdOutput() {
return hsvThresholdOutput;
}
/**
* This method is a generated getter for the output of a CV_erode.
* @return Mat output from CV_erode.
*/
public Mat cvErodeOutput() {
return cvErodeOutput;
}
/**
* This method is a generated getter for the output of a Mask.
* @return Mat output from Mask.
*/
public Mat maskOutput() {
return maskOutput;
}
/**
* This method is a generated getter for the output of a Find_Lines.
* @return ArrayList<Line> output from Find_Lines.
*/
public ArrayList<Line> findLinesOutput() {
return findLinesOutput;
}
/**
* Resizes an image.
* @param src The image to resize.
* @param dSize size to set the image.
* @param fx scale factor along X axis.
* @param fy scale factor along Y axis.
* @param interpolation type of interpolation to use.
* @param dst output image.
*/
private void cvResize(Mat src, Size dSize, double fx, double fy, int interpolation,
Mat dst) {
if (dSize==null) {
dSize = new Size(0,0);
}
Imgproc.resize(src, dst, dSize, fx, fy, interpolation);
}
/**
* Segment an image based on hue, saturation, and value ranges.
*
* @param input The image on which to perform the HSL threshold.
* @param hue The min and max hue
* @param sat The min and max saturation
* @param val The min and max value
* @param output The image in which to store the output.
*/
private void hsvThreshold(Mat input, double[] hue, double[] sat, double[] val,
Mat out) {
Imgproc.cvtColor(input, out, Imgproc.COLOR_BGR2HSV);
Core.inRange(out, new Scalar(hue[0], sat[0], val[0]),
new Scalar(hue[1], sat[1], val[1]), out);
}
/**
* Expands area of lower value in an image.
* @param src the Image to erode.
* @param kernel the kernel for erosion.
* @param anchor the center of the kernel.
* @param iterations the number of times to perform the erosion.
* @param borderType pixel extrapolation method.
* @param borderValue value to be used for a constant border.
* @param dst Output Image.
*/
private void cvErode(Mat src, Mat kernel, Point anchor, double iterations,
int borderType, Scalar borderValue, Mat dst) {
if (kernel == null) {
kernel = new Mat();
}
if (anchor == null) {
anchor = new Point(-1,-1);
}
if (borderValue == null) {
borderValue = new Scalar(-1);
}
Imgproc.erode(src, dst, kernel, anchor, (int)iterations, borderType, borderValue);
}
/**
* Filter out an area of an image using a binary mask.
* @param input The image on which the mask filters.
* @param mask The binary image that is used to filter.
* @param output The image in which to store the output.
*/
private void mask(Mat input, Mat mask, Mat output) {
mask.convertTo(mask, CvType.CV_8UC1);
Core.bitwise_xor(output, output, output);
input.copyTo(output, mask);
}
public static class Line {
public final double x1, y1, x2, y2;
public Line(double x1, double y1, double x2, double y2) {
this.x1 = x1;
this.y1 = y1;
this.x2 = x2;
this.y2 = y2; }
public double lengthSquared() {
return Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2); }
public double length() {
return Math.sqrt(lengthSquared());
}
public double angle() {
return Math.toDegrees(Math.atan2(y2 - y1, x2 - x1));}}
/**
* Finds all line segments in an image.
* @param input The image on which to perform the find lines.
* @param lineList The output where the lines are stored.
*/
private void findLines(Mat input, ArrayList<Line> lineList) {
final LineSegmentDetector lsd = Imgproc.createLineSegmentDetector();
final Mat lines = new Mat();
lineList.clear();
if (input.channels() == 1) {
lsd.detect(input, lines);
} else {
final Mat tmp = new Mat();
Imgproc.cvtColor(input, tmp, Imgproc.COLOR_BGR2GRAY);
lsd.detect(tmp, lines);
}
if (!lines.empty()) {
for (int i = 0; i < lines.rows(); i++) {
lineList.add(new Line(lines.get(i, 0)[0], lines.get(i, 0)[1],
lines.get(i, 0)[2], lines.get(i, 0)[3]));}}}}
```