eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Integrate with ielib

This commit is contained in:
Scott Barnes 2024-03-09 10:32:09 -06:00
parent 3dbbdeb30d
commit 5ff94b987b
5 changed files with 64 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java
View File

@ -1,12 +1,12 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_NAME;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.tearabite.ielib.localization.AprilTagPoseEstimator;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import org.firstinspires.ftc.teamcode.vision.Detection;
@ -17,30 +17,51 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
@Config
public class Camera {
public static float PROP_REJECTION_VERTICAL_UPPER = 175;
public static float PROP_REJECTION_VERTICAL_LOWER = 300;
private static final String REAR_WEBCAM_NAME = "rearWebcam";
public static final String FRONT_WEBCAM_NAME = "frontWebcam";
private static final Pose2d REAR_CAMERA_OFFSETS = new Pose2d(8.9, 1.5, Math.PI);
public static float PROP_REJECTION_VERTICAL_UPPER = 480f * 0.33f;
public static float PROP_REJECTION_VERTICAL_LOWER = 440;
private PropDetectionPipeline prop;
private AprilTagProcessor aprilTag;
private VisionPortal visionPortal;
private Telemetry telemetry;
private VisionPortal aprilTagPortal;
private VisionPortal propPortal;
private boolean initialized;
private AprilTagPoseEstimator aprilTagPoseEstimator;
public Camera(HardwareMap hardwareMap) {
this.telemetry = telemetry;
this.init(hardwareMap);
}
private void init(HardwareMap hardwareMap) {
this.aprilTagPoseEstimator = AprilTagPoseEstimator.builder()
.robotOffset(REAR_CAMERA_OFFSETS)
.build();
this.aprilTag = new AprilTagProcessor.Builder()
.setDrawAxes(true)
.setDrawCubeProjection(true)
.setDrawTagID(true)
.setDrawTagOutline(true)
.build();
int[] viewportIds = VisionPortal.makeMultiPortalView(2, VisionPortal.MultiPortalLayout.HORIZONTAL);
VisionPortal.Builder aprilTagVisionPortalBuilder = new VisionPortal.Builder();
aprilTagVisionPortalBuilder.setCamera(hardwareMap.get(WebcamName.class, REAR_WEBCAM_NAME));
aprilTagVisionPortalBuilder.setLiveViewContainerId(viewportIds[0]);
aprilTagVisionPortalBuilder.setAutoStopLiveView(true);
aprilTagVisionPortalBuilder.addProcessor(aprilTag);
this.aprilTagPortal = aprilTagVisionPortalBuilder.build();
this.prop = new PropDetectionPipeline();
this.visionPortal = VisionPortal.easyCreateWithDefaults(
hardwareMap.get(WebcamName.class, WEBCAM_NAME), aprilTag, prop);
VisionPortal.Builder propVisionPortalBuilder = new VisionPortal.Builder();
propVisionPortalBuilder.setCamera(hardwareMap.get(WebcamName.class, FRONT_WEBCAM_NAME));
propVisionPortalBuilder.setLiveViewContainerId(viewportIds[1]);
propVisionPortalBuilder.setAutoStopLiveView(true);
propVisionPortalBuilder.addProcessor(prop);
this.propPortal = propVisionPortalBuilder.build();
this.propPortal.resumeLiveView();
this.aprilTagPortal.resumeLiveView();
this.initialized = true;
}
@ -61,27 +82,6 @@ public class Camera {
return INVALID_DETECTION;
}
public AprilTagDetection getAprilTag(int id) {
return this.aprilTag.getDetections()
.stream()
.filter(x -> x.id == id)
.findFirst()
.orElse(null);
}
public double getDistanceToAprilTag(int id, double rejectAbove, double rejectBelow) {
for (int i = 0; i < 10; i++) {
AprilTagDetection aprilTagDetection = getAprilTag(id);
if (aprilTagDetection != null) {
if (aprilTagDetection.ftcPose.y < rejectAbove
&& aprilTagDetection.ftcPose.y > rejectBelow) {
return aprilTagDetection.ftcPose.y;
}
}
}
return Double.MAX_VALUE;
}
public void setAlliance(CenterStageCommon.Alliance alliance) {
this.prop.setAlliance(alliance);
}
@ -90,7 +90,17 @@ public class Camera {
return this.prop != null ? this.prop.getAlliance() : null;
}
public boolean isCameraReady() {
return this.visionPortal.getCameraState() == VisionPortal.CameraState.CAMERA_DEVICE_READY;
public AprilTagDetection getBestAprilTagDetection() {
return this.aprilTag.getDetections()
.stream().max((d1, d2) -> Float.compare(d2.decisionMargin, d1.decisionMargin))
.orElse(null);
}
public Pose2d estimatePoseFromAprilTag() {
AprilTagDetection detection = this.getBestAprilTagDetection();
if (detection == null) {
return null;
}
return this.aprilTagPoseEstimator.estimatePose(detection);
}
}

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@ -178,16 +179,12 @@ public class Robot {
}
}
public void update(double runTime) {
drive.update();
Pose2d estimatedPose = null;
if (camera != null) {
estimatedPose = this.camera.estimatePoseFromAprilTag();
}
drive.update(estimatedPose);
slides.update(runTime);
arm.update();
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java
View File

@ -205,7 +205,15 @@ public class SampleMecanumDrive extends MecanumDrive {
}
public void update() {
updatePoseEstimate();
this.update(null);
}
public void update(Pose2d guess) {
if (guess == null) {
updatePoseEstimate();
} else {
setPoseEstimate(guess);
}
DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
if (signal != null) setDriveSignal(signal);
}

2
build.dependencies.gradle
View File

@ -19,5 +19,7 @@ dependencies {
runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0'
implementation 'androidx.appcompat:appcompat:1.2.0'
implementation 'com.acmerobotics.dashboard:dashboard:0.4.12'
implementation project(':ielib-core')
implementation project(':ielib')
}

4
settings.gradle
View File

@ -2,3 +2,7 @@ include ':FtcRobotController'
include ':TeamCode'
include ':MeepMeepTesting'
include ':MeepMeepTesting'
include ':ielib-core'
project(':ielib-core').projectDir = new File('../ielib-core')
include ':ielib'
project(':ielib').projectDir = new File('../ielib')