mirror of
https://github.com/trc492/FtcTemplate.git
synced 2025-07-01 13:01:24 -07:00
0869026b76
Moved all auto tasks from autotasks to tasks. Added new Dashboard class. Added solvePnP support to vision. Added updateStatus to vision. Added updateStatus too FtcAuto, FtcTeleOp. Removed updateStatus from Robot.java. Added asymetric motion profile support to PurePursuit. Added updateStatus to RobotBase. Added solvePnp support for vision. Updated trclib, ftclib. Added acme FtcDashboard support. Added support to tune using acme FtcDashboard. Added maxDeceleration support in SpeedTest.
996 lines
38 KiB
Java
996 lines
38 KiB
Java
/*
|
|
* Copyright (c) 2023 Titan Robotics Club (http://www.titanrobotics.com)
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
|
|
package teamcode.vision;
|
|
|
|
import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
|
|
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
|
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
|
import org.firstinspires.ftc.vision.VisionProcessor;
|
|
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
|
|
import org.opencv.core.CvType;
|
|
import org.opencv.core.Mat;
|
|
import org.opencv.core.MatOfDouble;
|
|
import org.opencv.imgproc.Imgproc;
|
|
import org.openftc.easyopencv.OpenCvCamera;
|
|
import org.openftc.easyopencv.OpenCvCameraFactory;
|
|
import org.openftc.easyopencv.OpenCvCameraRotation;
|
|
|
|
import java.util.ArrayList;
|
|
|
|
import ftclib.drivebase.FtcRobotDrive;
|
|
import ftclib.robotcore.FtcOpMode;
|
|
import ftclib.vision.FtcCameraStreamProcessor;
|
|
import ftclib.vision.FtcEocvColorBlobProcessor;
|
|
import ftclib.vision.FtcLimelightVision;
|
|
import ftclib.vision.FtcRawEocvColorBlobPipeline;
|
|
import ftclib.vision.FtcRawEocvVision;
|
|
import ftclib.vision.FtcVision;
|
|
import ftclib.vision.FtcVisionAprilTag;
|
|
import ftclib.vision.FtcVisionEocvColorBlob;
|
|
import teamcode.Dashboard;
|
|
import teamcode.Robot;
|
|
import teamcode.RobotParams;
|
|
import teamcode.subsystems.LEDIndicator;
|
|
import trclib.dataprocessor.TrcUtil;
|
|
import trclib.pathdrive.TrcPose2D;
|
|
import trclib.pathdrive.TrcPose3D;
|
|
import trclib.robotcore.TrcDbgTrace;
|
|
import trclib.vision.TrcHomographyMapper;
|
|
import trclib.vision.TrcOpenCvColorBlobPipeline;
|
|
import trclib.vision.TrcOpenCvDetector;
|
|
import trclib.vision.TrcVisionTargetInfo;
|
|
|
|
/**
|
|
* This class implements AprilTag/Eocv/Limelight Vision for the game season. It creates and initializes all the vision
|
|
* target info as well as providing info for the robot, camera and the field. It also provides methods to get the
|
|
* location of the robot and detected targets.
|
|
*/
|
|
public class Vision
|
|
{
|
|
private final String moduleName = getClass().getSimpleName();
|
|
|
|
/**
|
|
* This class contains the parameters of the front camera.
|
|
*/
|
|
public static class FrontCamParams extends FtcRobotDrive.VisionInfo
|
|
{
|
|
public FrontCamParams()
|
|
{
|
|
camName = "Webcam 1";
|
|
camImageWidth = 640;
|
|
camImageHeight = 480;
|
|
camXOffset = -4.25; // Inches to the right from robot center
|
|
camYOffset = 5.5; // Inches forward from robot center
|
|
camZOffset = 10.608; // Inches up from the floor
|
|
camYaw = -2.0; // degrees clockwise from robot forward
|
|
camPitch = -32.346629699; // degrees up from horizontal
|
|
camRoll = 0.0;
|
|
camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
|
|
camOrientation = OpenCvCameraRotation.UPRIGHT;
|
|
// Homography: cameraRect in pixels, worldRect in inches
|
|
cameraRect = new TrcHomographyMapper.Rectangle(
|
|
14.0, 28.0, // Camera Top Left
|
|
612.0, 33.0, // Camera Top Right
|
|
56.0, 448.0, // Camera Bottom Left
|
|
581.0, 430.5); // Camera Bottom Right
|
|
worldRect = new TrcHomographyMapper.Rectangle(
|
|
-19.0, 37.5, // World Top Left
|
|
24.0, 37.5, // World Top Right
|
|
-4.75, 9.0, // World Bottom Left
|
|
6.25, 9.0); // World Bottom Right
|
|
} //FrontCamParams
|
|
} //class FrontCamParams
|
|
|
|
/**
|
|
* This class contains the parameters of the back camera.
|
|
*/
|
|
public static class BackCamParams extends FtcRobotDrive.VisionInfo
|
|
{
|
|
public BackCamParams()
|
|
{
|
|
camName = "Webcam 2";
|
|
camImageWidth = 640;
|
|
camImageHeight = 480;
|
|
camXOffset = 0.0; // Inches to the right from robot center
|
|
camYOffset = 2.0; // Inches forward from robot center
|
|
camZOffset = 9.75; // Inches up from the floor
|
|
camYaw = 0.0; // degrees clockwise from robot front
|
|
camPitch = 15.0; // degrees down from horizontal
|
|
camRoll = 0.0;
|
|
camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
|
|
camOrientation = OpenCvCameraRotation.UPRIGHT;
|
|
// Homography: cameraRect in pixels, worldRect in inches
|
|
cameraRect = new TrcHomographyMapper.Rectangle(
|
|
0.0, 120.0, // Camera Top Left
|
|
camImageWidth -1, 120.0, // Camera Top Right
|
|
0.0, camImageHeight - 1, // Camera Bottom Left
|
|
camImageWidth - 1, camImageHeight - 1); // Camera Bottom Right
|
|
worldRect = new TrcHomographyMapper.Rectangle(
|
|
-12.5626, 48.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Left
|
|
11.4375, 44.75 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Top Right
|
|
-2.5625, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset, // World Bottom Left
|
|
2.5626, 21.0 - RobotParams.Robot.ROBOT_LENGTH/2.0 - camYOffset); // World Bottom Right
|
|
} //BackCamParams
|
|
} //class BackCamParams
|
|
|
|
/**
|
|
* This class contains the parameters of the Limelight vision processor.
|
|
*/
|
|
public static class LimelightParams extends FtcRobotDrive.VisionInfo
|
|
{
|
|
public static final int NUM_PIPELINES = 4;
|
|
|
|
public LimelightParams()
|
|
{
|
|
camName = "Limelight3a";
|
|
camImageWidth = 640;
|
|
camImageHeight = 480;
|
|
camHFov = 80.0; // in degrees
|
|
camVFov = 56.0; // in degrees
|
|
camXOffset = 135.47*TrcUtil.INCHES_PER_MM; // Inches to the right from robot center
|
|
camYOffset = 2.073; // Inches forward from robot center
|
|
camZOffset = 10.758; // Inches up from the floor
|
|
camYaw = -3.438; // degrees clockwise from robot front
|
|
camPitch = 0.0; // degrees down from horizontal
|
|
camRoll = 0.0;
|
|
camPose = new TrcPose3D(camXOffset, camYOffset, camZOffset, camYaw, camPitch, camRoll);
|
|
} //LimelightParams
|
|
} //class LimelightParams
|
|
|
|
public enum ColorBlobType
|
|
{
|
|
RedBlob,
|
|
BlueBlob,
|
|
AnyColorBlob
|
|
} //enum ColorBlobType
|
|
|
|
// Warning: EOCV converts camera stream to RGBA whereas Desktop OpenCV converts it to BGRA. Therefore, the correct
|
|
// color conversion must be RGBA (or RGB) to whatever color space you want to convert.
|
|
//
|
|
// YCrCb Color Space.
|
|
private static final int colorConversion = Imgproc.COLOR_RGB2YCrCb;
|
|
private static final double[] redBlobColorThresholds = {10.0, 180.0, 170.0, 240.0, 80.0, 120.0};
|
|
private static final double[] blueBlobColorThresholds = {0.0, 180.0, 80.0, 150.0, 150.0, 200.0};
|
|
public static final TrcOpenCvColorBlobPipeline.FilterContourParams colorBlobFilterContourParams =
|
|
new TrcOpenCvColorBlobPipeline.FilterContourParams()
|
|
.setMinArea(500.0)
|
|
.setMinPerimeter(100.0)
|
|
.setWidthRange(10.0, 1000.0)
|
|
.setHeightRange(10.0, 1000.0)
|
|
.setSolidityRange(0.0, 100.0)
|
|
.setVerticesRange(0.0, 1000.0)
|
|
.setAspectRatioRange(0.5, 2.5);
|
|
private static final double objectWidth = 3.5;
|
|
private static final double objectHeight = 1.5;
|
|
// Logitech C920
|
|
private static final double fx = 622.001;
|
|
private static final double fy = 622.001;
|
|
private static final double cx = 319.803;
|
|
private static final double cy = 241.251;
|
|
private static final MatOfDouble distCoeffs = new MatOfDouble(0.1208, -0.261599, 0, 0, 0.10308, 0, 0, 0);
|
|
|
|
private final TrcDbgTrace tracer;
|
|
private final Robot robot;
|
|
private final WebcamName webcam1, webcam2;
|
|
private FtcRawEocvColorBlobPipeline rawColorBlobPipeline;
|
|
public FtcRawEocvVision rawColorBlobVision;
|
|
public FtcLimelightVision limelightVision;
|
|
private FtcCameraStreamProcessor cameraStreamProcessor;
|
|
public FtcVisionAprilTag aprilTagVision;
|
|
private AprilTagProcessor aprilTagProcessor;
|
|
public FtcVisionEocvColorBlob redBlobVision;
|
|
private FtcEocvColorBlobProcessor redBlobProcessor;
|
|
public FtcVisionEocvColorBlob blueBlobVision;
|
|
private FtcEocvColorBlobProcessor blueBlobProcessor;
|
|
public FtcVision vision;
|
|
|
|
/**
|
|
* Constructor: Create an instance of the object.
|
|
*
|
|
* @param robot specifies the robot object.
|
|
*/
|
|
public Vision(Robot robot)
|
|
{
|
|
FtcOpMode opMode = FtcOpMode.getInstance();
|
|
|
|
if (robot.robotInfo.webCam1 == null &&
|
|
(RobotParams.Preferences.useWebCam || RobotParams.Preferences.tuneColorBlobVision))
|
|
{
|
|
throw new IllegalArgumentException("Must provide valid WebCam 1 info.");
|
|
}
|
|
|
|
this.tracer = new TrcDbgTrace();
|
|
this.robot = robot;
|
|
webcam1 = robot.robotInfo.webCam1 != null?
|
|
opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam1.camName): null;
|
|
webcam2 = robot.robotInfo.webCam2 != null?
|
|
opMode.hardwareMap.get(WebcamName.class, robot.robotInfo.webCam2.camName): null;
|
|
Mat cameraMatrix = new Mat(3, 3, CvType.CV_64FC1);
|
|
cameraMatrix.put(0, 0,
|
|
fx, 0, cx,
|
|
0, fy, cy,
|
|
0, 0, 1);
|
|
// TuneColorBlobVision: must use webcam1.
|
|
if (RobotParams.Preferences.tuneColorBlobVision && webcam1 != null)
|
|
{
|
|
OpenCvCamera openCvCamera;
|
|
|
|
if (RobotParams.Preferences.showVisionView)
|
|
{
|
|
int cameraViewId = opMode.hardwareMap.appContext.getResources().getIdentifier(
|
|
"cameraMonitorViewId", "id", opMode.hardwareMap.appContext.getPackageName());
|
|
openCvCamera = OpenCvCameraFactory.getInstance().createWebcam(webcam1, cameraViewId);
|
|
}
|
|
else
|
|
{
|
|
openCvCamera = OpenCvCameraFactory.getInstance().createWebcam(webcam1);
|
|
}
|
|
|
|
if (RobotParams.Preferences.useCameraStreamProcessor)
|
|
{
|
|
com.acmerobotics.dashboard.FtcDashboard.getInstance().startCameraStream(openCvCamera, 0);
|
|
}
|
|
|
|
tracer.traceInfo(moduleName, "Starting RawEocvColorBlobVision...");
|
|
rawColorBlobPipeline = new FtcRawEocvColorBlobPipeline(
|
|
"rawColorBlobPipeline", colorConversion, Dashboard.Vision.colorThresholds,
|
|
Dashboard.Vision.filterContourParams, true, objectWidth, objectHeight,
|
|
RobotParams.Preferences.useSolvePnp? cameraMatrix: null, distCoeffs, robot.robotInfo.webCam1.camPose);
|
|
// By default, display original Mat.
|
|
rawColorBlobPipeline.setVideoOutput(0);
|
|
rawColorBlobPipeline.setAnnotateEnabled(true);
|
|
rawColorBlobVision = new FtcRawEocvVision(
|
|
"rawColorBlobVision", robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight,
|
|
null, null,
|
|
openCvCamera, robot.robotInfo.webCam1.camOrientation);
|
|
rawColorBlobVision.setFpsMeterEnabled(RobotParams.Preferences.showVisionStat);
|
|
setRawColorBlobVisionEnabled(false);
|
|
}
|
|
else
|
|
{
|
|
// LimelightVision (not a Vision Processor).
|
|
if (RobotParams.Preferences.useLimelightVision && robot.robotInfo.limelight != null)
|
|
{
|
|
limelightVision = new FtcLimelightVision(
|
|
robot.robotInfo.limelight.camName, robot.robotInfo.limelight.camPose, this::getTargetGroundOffset);
|
|
limelightVision.setPipeline(0);
|
|
}
|
|
// Creating Vision Processors for VisionPortal.
|
|
ArrayList<VisionProcessor> visionProcessorsList = new ArrayList<>();
|
|
|
|
if (RobotParams.Preferences.useCameraStreamProcessor)
|
|
{
|
|
cameraStreamProcessor = new FtcCameraStreamProcessor();
|
|
visionProcessorsList.add(cameraStreamProcessor);
|
|
com.acmerobotics.dashboard.FtcDashboard.getInstance().startCameraStream(cameraStreamProcessor, 0);
|
|
}
|
|
|
|
if (RobotParams.Preferences.useWebcamAprilTagVision)
|
|
{
|
|
tracer.traceInfo(moduleName, "Starting Webcam AprilTagVision...");
|
|
FtcVisionAprilTag.Parameters aprilTagParams = new FtcVisionAprilTag.Parameters()
|
|
.setDrawTagIdEnabled(true)
|
|
.setDrawTagOutlineEnabled(true)
|
|
.setDrawAxesEnabled(false)
|
|
.setDrawCubeProjectionEnabled(false)
|
|
.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES);
|
|
aprilTagVision = new FtcVisionAprilTag(aprilTagParams, AprilTagProcessor.TagFamily.TAG_36h11);
|
|
aprilTagProcessor = aprilTagVision.getVisionProcessor();
|
|
visionProcessorsList.add(aprilTagProcessor);
|
|
}
|
|
|
|
if (RobotParams.Preferences.useColorBlobVision && robot.robotInfo.webCam1 != null)
|
|
{
|
|
Mat camMatrix;
|
|
TrcHomographyMapper.Rectangle camRect, worldRect;
|
|
|
|
if (RobotParams.Preferences.useSolvePnp)
|
|
{
|
|
camMatrix = cameraMatrix;
|
|
camRect = null;
|
|
worldRect = null;
|
|
}
|
|
else
|
|
{
|
|
camMatrix = null;
|
|
camRect = robot.robotInfo.webCam1.cameraRect;
|
|
worldRect = robot.robotInfo.webCam1.worldRect;
|
|
}
|
|
|
|
tracer.traceInfo(moduleName, "Starting Webcam ColorBlobVision...");
|
|
redBlobVision = new FtcVisionEocvColorBlob(
|
|
LEDIndicator.RED_BLOB, colorConversion, redBlobColorThresholds, colorBlobFilterContourParams,
|
|
true, objectWidth, objectHeight, camMatrix, distCoeffs, robot.robotInfo.webCam1.camPose, camRect,
|
|
worldRect, true);
|
|
redBlobProcessor = redBlobVision.getVisionProcessor();
|
|
visionProcessorsList.add(redBlobProcessor);
|
|
|
|
blueBlobVision = new FtcVisionEocvColorBlob(
|
|
LEDIndicator.BLUE_BLOB, colorConversion, blueBlobColorThresholds, colorBlobFilterContourParams,
|
|
true, objectWidth, objectHeight, camMatrix, distCoeffs, robot.robotInfo.webCam1.camPose, camRect,
|
|
worldRect, true);
|
|
blueBlobProcessor = blueBlobVision.getVisionProcessor();
|
|
visionProcessorsList.add(blueBlobProcessor);
|
|
}
|
|
|
|
if (!visionProcessorsList.isEmpty())
|
|
{
|
|
VisionProcessor[] visionProcessors = new VisionProcessor[visionProcessorsList.size()];
|
|
visionProcessorsList.toArray(visionProcessors);
|
|
if (RobotParams.Preferences.useWebCam)
|
|
{
|
|
// Use USB webcams.
|
|
vision = new FtcVision(
|
|
webcam1, webcam2, robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight,
|
|
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat,
|
|
visionProcessors);
|
|
}
|
|
else
|
|
{
|
|
// Use phone camera.
|
|
vision = new FtcVision(
|
|
RobotParams.Preferences.useBuiltinCamBack?
|
|
BuiltinCameraDirection.BACK: BuiltinCameraDirection.FRONT,
|
|
robot.robotInfo.webCam1.camImageWidth, robot.robotInfo.webCam1.camImageHeight,
|
|
RobotParams.Preferences.showVisionView, RobotParams.Preferences.showVisionStat,
|
|
visionProcessors);
|
|
}
|
|
|
|
// Disable all vision until they are needed.
|
|
for (VisionProcessor processor: visionProcessors)
|
|
{
|
|
vision.setProcessorEnabled(processor, false);
|
|
}
|
|
}
|
|
}
|
|
} //Vision
|
|
|
|
/**
|
|
* This method closes the vision portal and is normally called at the end of an opmode.
|
|
*/
|
|
public void close()
|
|
{
|
|
if (vision != null)
|
|
{
|
|
vision.close();
|
|
}
|
|
} //close
|
|
|
|
/**
|
|
* This method enables/disables FPS meter on the viewport.
|
|
*
|
|
* @param enabled specifies true to enable FPS meter, false to disable.
|
|
*/
|
|
public void setFpsMeterEnabled(boolean enabled)
|
|
{
|
|
if (rawColorBlobVision != null)
|
|
{
|
|
rawColorBlobVision.setFpsMeterEnabled(enabled);
|
|
}
|
|
else if (vision != null)
|
|
{
|
|
vision.setFpsMeterEnabled(enabled);
|
|
}
|
|
} //setFpsMeterEnabled
|
|
|
|
/**
|
|
* This method returns the front webcam.
|
|
*
|
|
* @return front webcam.
|
|
*/
|
|
public WebcamName getFrontWebcam()
|
|
{
|
|
return webcam1;
|
|
} //getFrontWebcam
|
|
|
|
/**
|
|
* This method returns the rear webcam.
|
|
*
|
|
* @return rear webcam.
|
|
*/
|
|
public WebcamName getRearWebcam()
|
|
{
|
|
return webcam2;
|
|
} //getRearWebcam
|
|
|
|
/**
|
|
* This method returns the active camera if we have two webcams.
|
|
*
|
|
* @return active camera.
|
|
*/
|
|
public WebcamName getActiveWebcam()
|
|
{
|
|
return vision.getActiveWebcam();
|
|
} //getActiveWebcam
|
|
|
|
/**
|
|
* This method sets the active webcam.
|
|
*
|
|
* @param webcam specifies the webcam to be set as active.
|
|
*/
|
|
public void setActiveWebcam(WebcamName webcam)
|
|
{
|
|
vision.setActiveWebcam(webcam);
|
|
} //setActiveWebcam
|
|
|
|
/**
|
|
* This method displays the exposure settings on the dashboard. This helps tuning camera exposure.
|
|
*
|
|
* @param lineNum specifies the dashboard line number to display the info.
|
|
*/
|
|
public void displayExposureSettings(int lineNum)
|
|
{
|
|
long[] exposureSetting = vision.getExposureSetting();
|
|
long currExposure = vision.getCurrentExposure();
|
|
int[] gainSetting = vision.getGainSetting();
|
|
int currGain = vision.getCurrentGain();
|
|
|
|
if (exposureSetting != null && gainSetting != null)
|
|
{
|
|
robot.dashboard.displayPrintf(
|
|
lineNum, "Exp: %d (%d:%d), Gain: %d (%d:%d)",
|
|
currExposure, exposureSetting[0], exposureSetting[1], currGain, gainSetting[0], gainSetting[1]);
|
|
}
|
|
} //displayExposureSettings
|
|
|
|
/**
|
|
* This method returns the color threshold values of rawColorBlobVision.
|
|
*
|
|
* @return array of color threshold values.
|
|
*/
|
|
public double[] getRawColorBlobThresholds()
|
|
{
|
|
return rawColorBlobPipeline != null? rawColorBlobPipeline.getColorThresholds(): null;
|
|
} //getRawColorBlobThresholds
|
|
|
|
/**
|
|
* This method sets the color threshold values of rawColorBlobVision.
|
|
*
|
|
* @param colorThresholds specifies an array of color threshold values.
|
|
*/
|
|
public void setRawColorBlobThresholds(double... colorThresholds)
|
|
{
|
|
if (rawColorBlobPipeline != null)
|
|
{
|
|
rawColorBlobPipeline.setColorThresholds(colorThresholds);
|
|
}
|
|
} //setRawColorBlobThresholds
|
|
|
|
/**
|
|
* This method enables/disables raw ColorBlob vision.
|
|
*
|
|
* @param enabled specifies true to enable, false to disable.
|
|
*/
|
|
public void setRawColorBlobVisionEnabled(boolean enabled)
|
|
{
|
|
if (rawColorBlobVision != null)
|
|
{
|
|
rawColorBlobVision.setPipeline(enabled? rawColorBlobPipeline: null);
|
|
}
|
|
} //setRawColorBlobVisionEnabled
|
|
|
|
/**
|
|
* This method checks if raw ColorBlob vision is enabled.
|
|
*
|
|
* @return true if enabled, false if disabled.
|
|
*/
|
|
public boolean isRawColorBlobVisionEnabled()
|
|
{
|
|
return rawColorBlobVision != null && rawColorBlobVision.getPipeline() != null;
|
|
} //isRawColorBlobVisionEnabled
|
|
|
|
/**
|
|
* This method calls RawColorBlob vision to detect the color blob for color threshold tuning.
|
|
*
|
|
* @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing.
|
|
* @return detected raw color blob object info.
|
|
*/
|
|
public TrcVisionTargetInfo<TrcOpenCvDetector.DetectedObject<?>> getDetectedRawColorBlob(int lineNum)
|
|
{
|
|
TrcVisionTargetInfo<TrcOpenCvDetector.DetectedObject<?>> colorBlobInfo =
|
|
rawColorBlobVision != null? rawColorBlobVision.getBestDetectedTargetInfo(null, null, 0.0, 0.0): null;
|
|
|
|
if (cameraStreamProcessor != null && colorBlobInfo != null)
|
|
{
|
|
cameraStreamProcessor.addRectInfo(
|
|
colorBlobInfo.detectedObj.label, colorBlobInfo.detectedObj.getRotatedRectVertices());
|
|
}
|
|
|
|
if (colorBlobInfo != null && robot.ledIndicator != null)
|
|
{
|
|
robot.ledIndicator.setDetectedPattern(colorBlobInfo.detectedObj.label);
|
|
}
|
|
|
|
if (lineNum != -1)
|
|
{
|
|
robot.dashboard.displayPrintf(
|
|
lineNum, "RawColorBlob: %s, heading=%.3f",
|
|
colorBlobInfo != null? colorBlobInfo: "Not found.",
|
|
robot.robotDrive != null? robot.robotDrive.driveBase.getHeading(): 0.0);
|
|
}
|
|
|
|
return colorBlobInfo;
|
|
} //getDetectedRawColorBlob
|
|
|
|
/**
|
|
* This method enables/disables Limelight vision for the specified pipeline.
|
|
*
|
|
* @param pipelineIndex specifies the limelight pipeline index to be selected, ignore if disabled.
|
|
* @param enabled specifies true to enable, false to disable.
|
|
*/
|
|
public void setLimelightVisionEnabled(int pipelineIndex, boolean enabled)
|
|
{
|
|
if (limelightVision != null)
|
|
{
|
|
if (enabled)
|
|
{
|
|
limelightVision.setPipeline(pipelineIndex);
|
|
}
|
|
limelightVision.setVisionEnabled(enabled);
|
|
tracer.traceInfo(moduleName, "Pipeline %d is %s: running=%s",
|
|
pipelineIndex, enabled? "enabled": "disabled", limelightVision.limelight.isRunning());
|
|
}
|
|
} //setLimelightVisionEnabled
|
|
|
|
/**
|
|
* This method checks if Limelight vision is enabled.
|
|
*
|
|
* @return true if enabled, false if disabled.
|
|
*/
|
|
public boolean isLimelightVisionEnabled()
|
|
{
|
|
return limelightVision != null && limelightVision.isVisionEnabled();
|
|
} //isLimelightVisionEnabled
|
|
|
|
/**
|
|
* This method calls Limelight vision to detect the object.
|
|
*
|
|
* @param resultType specifies the result type to look for.
|
|
* @param label specifies the detected object label, can be null to match any label.
|
|
* @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing.
|
|
* @return detected Limelight object info.
|
|
*/
|
|
public TrcVisionTargetInfo<FtcLimelightVision.DetectedObject> getLimelightDetectedObject(
|
|
FtcLimelightVision.ResultType resultType, String label, int lineNum)
|
|
{
|
|
TrcVisionTargetInfo<FtcLimelightVision.DetectedObject> limelightInfo = null;
|
|
|
|
if (limelightVision != null)
|
|
{
|
|
String objectName = null;
|
|
int pipelineIndex = -1;
|
|
Double robotHeading = robot.robotDrive != null? robot.robotDrive.driveBase.getHeading(): null;
|
|
|
|
limelightInfo = limelightVision.getBestDetectedTargetInfo(resultType, label, robotHeading, null);
|
|
if (limelightInfo != null)
|
|
{
|
|
pipelineIndex = limelightVision.getPipeline();
|
|
switch (pipelineIndex)
|
|
{
|
|
case 0:
|
|
objectName = LEDIndicator.APRIL_TAG;
|
|
break;
|
|
|
|
case 1:
|
|
objectName = LEDIndicator.RED_BLOB;
|
|
break;
|
|
|
|
case 2:
|
|
objectName = LEDIndicator.BLUE_BLOB;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (objectName != null && robot.ledIndicator != null)
|
|
{
|
|
robot.ledIndicator.setDetectedPattern(objectName);
|
|
}
|
|
|
|
if (lineNum != -1)
|
|
{
|
|
robot.dashboard.displayPrintf(
|
|
lineNum, "%s(%d): %s",
|
|
objectName, pipelineIndex, limelightInfo != null? limelightInfo: "Not found.");
|
|
}
|
|
}
|
|
|
|
return limelightInfo;
|
|
} //getLimelightDetectedObject
|
|
|
|
/**
|
|
* This method enables/disables the Vision Processor.
|
|
*
|
|
* @param processor specifies the vision processor to enable/disable.
|
|
* @param enabled specifies true to enable, false to disable.
|
|
*/
|
|
public void setVisionProcessorEnabled(VisionProcessor processor, boolean enabled)
|
|
{
|
|
if (processor != null)
|
|
{
|
|
vision.setProcessorEnabled(processor, enabled);
|
|
}
|
|
} //setVisionProcessorEnabled
|
|
|
|
/**
|
|
* This method checks if the Vision Processor is enabled.
|
|
*
|
|
* @param processor specifies the vision processor to enable/disable.
|
|
* @return true if enabled, false if disabled.
|
|
*/
|
|
public boolean isVisionProcessorEnabled(VisionProcessor processor)
|
|
{
|
|
return processor != null && vision.isVisionProcessorEnabled(processor);
|
|
} //isVisionProcessorEnabled
|
|
|
|
/**
|
|
* This method enables/disables the CameraStream processor.
|
|
*
|
|
* @param enabled specifies true to enable, false to disable.
|
|
*/
|
|
public void setCameraStreamEnabled(boolean enabled)
|
|
{
|
|
if (vision != null && cameraStreamProcessor != null)
|
|
{
|
|
cameraStreamProcessor.setCameraStreamEnabled(vision, enabled);
|
|
}
|
|
} //setCameraStreamEnabled
|
|
|
|
/**
|
|
* This method checks if the CameraStream processor is enabled.
|
|
*
|
|
* @return true if enabled, false if disabled.
|
|
*/
|
|
public boolean isCameraStreamEnabled()
|
|
{
|
|
return cameraStreamProcessor != null && cameraStreamProcessor.isCameraStreamEnabled();
|
|
} //isAprilTagVisionEnabled
|
|
|
|
/**
|
|
* This method enables/disables AprilTag vision.
|
|
*
|
|
* @param enabled specifies true to enable, false to disable.
|
|
*/
|
|
public void setAprilTagVisionEnabled(boolean enabled)
|
|
{
|
|
setVisionProcessorEnabled(aprilTagProcessor, enabled);
|
|
} //setAprilTagVisionEnabled
|
|
|
|
/**
|
|
* This method checks if AprilTag vision is enabled.
|
|
*
|
|
* @return true if enabled, false if disabled.
|
|
*/
|
|
public boolean isAprilTagVisionEnabled()
|
|
{
|
|
return isVisionProcessorEnabled(aprilTagProcessor);
|
|
} //isAprilTagVisionEnabled
|
|
|
|
/**
|
|
* This method calls AprilTag vision to detect the AprilTag object.
|
|
*
|
|
* @param id specifies the AprilTag ID to look for, null if match to any ID.
|
|
* @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing.
|
|
* @return detected AprilTag object info.
|
|
*/
|
|
public TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> getDetectedAprilTag(Integer id, int lineNum)
|
|
{
|
|
TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> aprilTagInfo =
|
|
aprilTagVision.getBestDetectedTargetInfo(id, null);
|
|
|
|
if (cameraStreamProcessor != null && aprilTagInfo != null)
|
|
{
|
|
cameraStreamProcessor.addRectInfo(
|
|
Integer.toString(aprilTagInfo.detectedObj.aprilTagDetection.id),
|
|
aprilTagInfo.detectedObj.getRotatedRectVertices());
|
|
}
|
|
|
|
if (aprilTagInfo != null && robot.ledIndicator != null)
|
|
{
|
|
robot.ledIndicator.setDetectedPattern(LEDIndicator.APRIL_TAG);
|
|
}
|
|
|
|
if (lineNum != -1)
|
|
{
|
|
robot.dashboard.displayPrintf(
|
|
lineNum, "%s: %s", LEDIndicator.APRIL_TAG, aprilTagInfo != null? aprilTagInfo : "Not found.");
|
|
}
|
|
|
|
return aprilTagInfo;
|
|
} //getDetectedAprilTag
|
|
|
|
/**
|
|
* This method calculates the robot's absolute field location with the detected AprilTagInfo.
|
|
*
|
|
* @param aprilTagInfo specifies the detected AprilTag info.
|
|
* @return robot field location.
|
|
*/
|
|
public TrcPose2D getRobotFieldPose(TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> aprilTagInfo)
|
|
{
|
|
TrcPose2D robotPose = null;
|
|
|
|
if (aprilTagInfo != null)
|
|
{
|
|
TrcPose2D aprilTagPose =
|
|
RobotParams.Game.APRILTAG_POSES[aprilTagInfo.detectedObj.aprilTagDetection.id - 1];
|
|
TrcPose2D cameraPose = aprilTagPose.subtractRelativePose(aprilTagInfo.objPose);
|
|
robotPose = cameraPose.subtractRelativePose(
|
|
new TrcPose2D(robot.robotInfo.webCam1.camXOffset, robot.robotInfo.webCam1.camYOffset,
|
|
robot.robotInfo.webCam1.camYaw));
|
|
tracer.traceInfo(
|
|
moduleName,
|
|
"AprilTagId=" + aprilTagInfo.detectedObj.aprilTagDetection.id +
|
|
", aprilTagFieldPose=" + aprilTagPose +
|
|
", aprilTagPoseFromCamera=" + aprilTagInfo.objPose +
|
|
", cameraPose=" + cameraPose +
|
|
", robotPose=%s" + robotPose);
|
|
}
|
|
|
|
return robotPose;
|
|
} //getRobotFieldPose
|
|
|
|
/**
|
|
* This method uses vision to find an AprilTag and uses the AprilTag's absolute field location and its relative
|
|
* position from the camera to calculate the robot's absolute field location.
|
|
*
|
|
* @return robot field location.
|
|
*/
|
|
public TrcPose2D getRobotFieldPose()
|
|
{
|
|
TrcPose2D robotPose = null;
|
|
|
|
if (isLimelightVisionEnabled())
|
|
{
|
|
TrcVisionTargetInfo<FtcLimelightVision.DetectedObject> aprilTagInfo =
|
|
getLimelightDetectedObject(FtcLimelightVision.ResultType.Fiducial, null, -1);
|
|
|
|
if (aprilTagInfo != null)
|
|
{
|
|
robotPose = aprilTagInfo.detectedObj.robotPose;
|
|
}
|
|
}
|
|
else if (isAprilTagVisionEnabled())
|
|
{
|
|
// Find any AprilTag in view.
|
|
TrcVisionTargetInfo<FtcVisionAprilTag.DetectedObject> aprilTagInfo = getDetectedAprilTag(null, -1);
|
|
|
|
if (aprilTagInfo != null)
|
|
{
|
|
robotPose = getRobotFieldPose(aprilTagInfo);
|
|
}
|
|
}
|
|
|
|
return robotPose;
|
|
} //getRobotFieldPose
|
|
|
|
/**
|
|
* This method enables/disables vision for the specified color blob type.
|
|
*
|
|
* @param colorBlobType specifies the color blob type to be detected.
|
|
* @param enabled specifies true to enable, false to disable.
|
|
*/
|
|
public void setColorBlobVisionEnabled(ColorBlobType colorBlobType, boolean enabled)
|
|
{
|
|
switch (colorBlobType)
|
|
{
|
|
case RedBlob:
|
|
setVisionProcessorEnabled(redBlobProcessor, enabled);
|
|
break;
|
|
|
|
case BlueBlob:
|
|
setVisionProcessorEnabled(blueBlobProcessor, enabled);
|
|
break;
|
|
|
|
case AnyColorBlob:
|
|
setVisionProcessorEnabled(redBlobProcessor, enabled);
|
|
setVisionProcessorEnabled(blueBlobProcessor, enabled);
|
|
break;
|
|
}
|
|
} //setColorBlobVisionEnabled
|
|
|
|
/**
|
|
* This method checks if vision is enabled for the specified color blob type.
|
|
*
|
|
* @param colorBlobType specifies the color blob type to be detected.
|
|
* @return true if enabled, false if disabled.
|
|
*/
|
|
public boolean isColorBlobVisionEnabled(ColorBlobType colorBlobType)
|
|
{
|
|
boolean enabled = false;
|
|
|
|
switch (colorBlobType)
|
|
{
|
|
case RedBlob:
|
|
enabled = isVisionProcessorEnabled(redBlobProcessor);
|
|
break;
|
|
|
|
case BlueBlob:
|
|
enabled = isVisionProcessorEnabled(blueBlobProcessor);
|
|
break;
|
|
|
|
case AnyColorBlob:
|
|
enabled = isVisionProcessorEnabled(redBlobProcessor) || isVisionProcessorEnabled(blueBlobProcessor);
|
|
break;
|
|
}
|
|
|
|
return enabled;
|
|
} //isColorBlobVisionEnabled
|
|
|
|
/**
|
|
* This method calls ColorBlob vision to detect the specified color blob object.
|
|
*
|
|
* @param colorBlobType specifies the color blob type to be detected.
|
|
* @param groundOffset specifies the ground offset of the detected sample.
|
|
* @param lineNum specifies the dashboard line number to display the detected object info, -1 to disable printing.
|
|
* @return detected color blob object info.
|
|
*/
|
|
public TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> getDetectedColorBlob(
|
|
ColorBlobType colorBlobType, double groundOffset, int lineNum)
|
|
{
|
|
TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> colorBlobInfo = null;
|
|
|
|
switch (colorBlobType)
|
|
{
|
|
case RedBlob:
|
|
colorBlobInfo = redBlobVision != null? redBlobVision.getBestDetectedTargetInfo(
|
|
null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset): null;
|
|
break;
|
|
|
|
case BlueBlob:
|
|
colorBlobInfo = blueBlobVision != null? blueBlobVision.getBestDetectedTargetInfo(
|
|
null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset): null;
|
|
break;
|
|
|
|
case AnyColorBlob:
|
|
ArrayList<TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject>> colorBlobList =
|
|
new ArrayList<>();
|
|
|
|
colorBlobInfo = redBlobVision != null ? redBlobVision.getBestDetectedTargetInfo(
|
|
null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset) : null;
|
|
if (colorBlobInfo != null)
|
|
{
|
|
colorBlobList.add(colorBlobInfo);
|
|
}
|
|
|
|
colorBlobInfo = blueBlobVision != null ? blueBlobVision.getBestDetectedTargetInfo(
|
|
null, this::compareDistance, groundOffset, robot.robotInfo.webCam1.camZOffset) : null;
|
|
if (colorBlobInfo != null)
|
|
{
|
|
colorBlobList.add(colorBlobInfo);
|
|
}
|
|
|
|
if (!colorBlobList.isEmpty())
|
|
{
|
|
if (colorBlobList.size() > 1)
|
|
{
|
|
colorBlobList.sort(this::compareDistance);
|
|
}
|
|
colorBlobInfo = colorBlobList.get(0);
|
|
}
|
|
break;
|
|
}
|
|
|
|
if (cameraStreamProcessor != null && colorBlobInfo != null)
|
|
{
|
|
cameraStreamProcessor.addRectInfo(
|
|
colorBlobInfo.detectedObj.label, colorBlobInfo.detectedObj.getRotatedRectVertices());
|
|
}
|
|
|
|
if (colorBlobInfo != null && robot.ledIndicator != null)
|
|
{
|
|
robot.ledIndicator.setDetectedPattern(colorBlobInfo.detectedObj.label);
|
|
}
|
|
|
|
if (lineNum != -1)
|
|
{
|
|
if (colorBlobInfo != null)
|
|
{
|
|
robot.dashboard.displayPrintf(lineNum, "%s: %s", colorBlobInfo.detectedObj.label, colorBlobInfo);
|
|
}
|
|
else
|
|
{
|
|
robot.dashboard.displayPrintf(lineNum, "No colorblob found.");
|
|
}
|
|
}
|
|
|
|
return colorBlobInfo;
|
|
} //getDetectedColorBlob
|
|
|
|
/**
|
|
* This method returns the target Z offset from ground.
|
|
*
|
|
* @param resultType specifies the detected object result type.
|
|
* @return target ground offset.
|
|
*/
|
|
private double getTargetGroundOffset(FtcLimelightVision.ResultType resultType)
|
|
{
|
|
double offset = 0.0;
|
|
|
|
if (resultType == FtcLimelightVision.ResultType.Fiducial)
|
|
{
|
|
offset = 5.75;
|
|
}
|
|
else if (resultType == FtcLimelightVision.ResultType.Python)
|
|
{
|
|
offset = 10.0;
|
|
}
|
|
|
|
return offset;
|
|
} //getTargetGroundOffset
|
|
|
|
/**
|
|
* This method is called by the Arrays.sort to sort the target object by increasing distance.
|
|
*
|
|
* @param a specifies the first target
|
|
* @param b specifies the second target.
|
|
* @return negative value if a has closer distance than b, 0 if a and b have equal distances, positive value
|
|
* if a has higher distance than b.
|
|
*/
|
|
private int compareDistance(
|
|
TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> a,
|
|
TrcVisionTargetInfo<TrcOpenCvColorBlobPipeline.DetectedObject> b)
|
|
{
|
|
return (int)((a.objPose.y - b.objPose.y)*100);
|
|
} //compareDistance
|
|
|
|
/**
|
|
* This method update the dashboard with vision status.
|
|
*
|
|
* @param lineNum specifies the starting line number to print the subsystem status.
|
|
* @return updated line number for the next subsystem to print.
|
|
*/
|
|
public int updateStatus(int lineNum)
|
|
{
|
|
if (rawColorBlobVision != null)
|
|
{
|
|
lineNum = rawColorBlobVision.updateStatus(lineNum);
|
|
}
|
|
|
|
if (limelightVision != null)
|
|
{
|
|
lineNum = limelightVision.updateStatus(lineNum);
|
|
}
|
|
|
|
if (aprilTagVision != null)
|
|
{
|
|
lineNum = aprilTagVision.updateStatus(lineNum);
|
|
}
|
|
|
|
if (redBlobVision != null)
|
|
{
|
|
lineNum = redBlobVision.updateStatus(lineNum);
|
|
}
|
|
|
|
if (blueBlobVision != null)
|
|
{
|
|
lineNum = blueBlobVision.updateStatus(lineNum);
|
|
}
|
|
|
|
return lineNum;
|
|
} //updateStatus
|
|
|
|
} //class Vision
|