From eaaaa7122a4579d7b638110abc4c6cf6616adf60 Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Sun, 13 Oct 2024 20:45:41 -0400 Subject: [PATCH] Add the thing --- .vscode/settings.json | 3 + .../org/team340/lib/swerve/SwerveAPI.java | 20 +- .../team340/lib/swerve/SwerveAPILogger.java | 4 +- .../org/team340/lib/swerve/SwerveState.java | 4 + .../org/team340/robot/subsystems/Swerve.java | 119 ++++++++- .../team340/robot/tagslam/GtsamInterface.java | 231 ++++++++++++++++++ .../team340/robot/tagslam/TagDetection.java | 17 ++ .../robot/tagslam/TagDetectionStruct.java | 51 ++++ vendordeps/photonlib.json | 57 +++++ 9 files changed, 495 insertions(+), 11 deletions(-) create mode 100644 src/main/java/org/team340/robot/tagslam/GtsamInterface.java create mode 100644 src/main/java/org/team340/robot/tagslam/TagDetection.java create mode 100644 src/main/java/org/team340/robot/tagslam/TagDetectionStruct.java create mode 100644 vendordeps/photonlib.json diff --git a/.vscode/settings.json b/.vscode/settings.json index 7afcde6..5ec2709 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -39,6 +39,8 @@ "Accl", "Accum", "ADIS", + "Backleft", + "Backright", "Bezier", "Botpose", "Brushless", @@ -69,6 +71,7 @@ "Gradlew", "Grav", "GRRDashboard", + "GTSAM", "Holonomic", "Integ", "Interpolatable", diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPI.java b/src/main/java/org/team340/lib/swerve/SwerveAPI.java index ac09419..b8bd4ba 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveAPI.java +++ b/src/main/java/org/team340/lib/swerve/SwerveAPI.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; @@ -31,6 +32,8 @@ public class SwerveAPI implements AutoCloseable { + public final SwerveState state; + final SwerveIMU imu; final SwerveModule[] modules; final SwerveConfig config; @@ -39,8 +42,8 @@ public class SwerveAPI implements AutoCloseable { private final double farthestModule; private final Translation2d[] moduleLocations; private final SwerveModuleState[] lockedStates; + private final SwerveDriveWheelPositions lastPositions; - private final SwerveState state; private final SwerveDriveKinematics kinematics; private final SwerveDrivePoseEstimator poseEstimator; @@ -59,15 +62,18 @@ public SwerveAPI(SwerveConfig config) { modules = new SwerveModule[moduleCount]; moduleLocations = new Translation2d[moduleCount]; lockedStates = new SwerveModuleState[moduleCount]; + var lastModulePositions = new SwerveModulePosition[moduleCount]; double farthest = 0.0; for (int i = 0; i < moduleCount; i++) { var moduleConfig = config.modules[i]; modules[i] = new SwerveModule(config, moduleConfig); moduleLocations[i] = moduleConfig.location; lockedStates[i] = new SwerveModuleState(0.0, moduleLocations[i].getAngle()); + lastModulePositions[i] = new SwerveModulePosition(); farthest = Math.max(farthest, moduleLocations[i].getNorm()); } + lastPositions = new SwerveDriveWheelPositions(lastModulePositions); farthestModule = farthest; state = new SwerveState(modules); @@ -85,19 +91,16 @@ public SwerveAPI(SwerveConfig config) { odometryThread = new SwerveOdometryThread(); } - /** - * Gets the current state of the robot's swerve drivetrain. - */ - public SwerveState getState() { - return state; - } - /** * Refreshes inputs from all swerve hardware. This must be called periodically * in order for the API to function. Typically, this method is called at the * start of the swerve subsystem's {@code periodic()} method. */ public void refresh() { + for (int i = 0; i < moduleCount; i++) { + Math2.copyInto(state.modules.positions[i], lastPositions.positions[i]); + } + odometryMutex.lock(); try { odometryThread.run(true); @@ -120,6 +123,7 @@ public void refresh() { state.pitch = imu.getPitch(); state.roll = imu.getRoll(); + state.twist = kinematics.toTwist2d(lastPositions, new SwerveDriveWheelPositions(state.modules.positions)); state.speeds = kinematics.toChassisSpeeds(state.modules.states); state.velocity = Math.hypot(state.speeds.vxMetersPerSecond, state.speeds.vyMetersPerSecond); diff --git a/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java b/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java index 822fadd..4f36eaf 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java +++ b/src/main/java/org/team340/lib/swerve/SwerveAPILogger.java @@ -7,6 +7,7 @@ import edu.wpi.first.epilogue.logging.errors.ErrorHandler; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -20,7 +21,7 @@ public SwerveAPILogger() { @Override public void update(DataLogger logger, SwerveAPI swerveAPI) { - logState(logger.getSubLogger("state"), swerveAPI.getState()); + logState(logger.getSubLogger("state"), swerveAPI.state); var hardwareLogger = logger.getSubLogger("hardware"); ErrorHandler errorHandler = Epilogue.getConfig().errorHandler; @@ -37,6 +38,7 @@ private void logState(DataLogger logger, SwerveState state) { logger.log("pitch", state.pitch, Rotation2d.struct); logger.log("roll", state.roll, Rotation2d.struct); logger.log("pose", state.pose, Pose2d.struct); + logger.log("twist", state.twist, Twist2d.struct); logger.log("speeds", state.speeds, ChassisSpeeds.struct); logger.log("targetSpeeds", state.targetSpeeds, ChassisSpeeds.struct); logger.log("velocity", state.velocity); diff --git a/src/main/java/org/team340/lib/swerve/SwerveState.java b/src/main/java/org/team340/lib/swerve/SwerveState.java index 1f86a0d..d3a5d54 100644 --- a/src/main/java/org/team340/lib/swerve/SwerveState.java +++ b/src/main/java/org/team340/lib/swerve/SwerveState.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -65,6 +66,8 @@ private Odometry() {} public Rotation2d roll; /** The current blue origin relative pose of the robot. */ public Pose2d pose; + /** A {@link Twist2d} representing the robot's movement since the last loop. */ + public Twist2d twist; /** The current measured robot-relative speeds. */ public ChassisSpeeds speeds; /** The target robot-relative speeds when using {@code applySpeeds()}. */ @@ -78,6 +81,7 @@ private Odometry() {} pitch = Math2.kZeroRotation2d; roll = Math2.kZeroRotation2d; pose = Math2.kZeroPose2d; + twist = new Twist2d(); speeds = new ChassisSpeeds(); targetSpeeds = new ChassisSpeeds(); } diff --git a/src/main/java/org/team340/robot/subsystems/Swerve.java b/src/main/java/org/team340/robot/subsystems/Swerve.java index ba462e1..85bd850 100644 --- a/src/main/java/org/team340/robot/subsystems/Swerve.java +++ b/src/main/java/org/team340/robot/subsystems/Swerve.java @@ -1,13 +1,33 @@ package org.team340.robot.subsystems; import com.revrobotics.CANSparkLowLevel.MotorType; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.geometry.Twist3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.wpilibj.ADIS16470_IMU.CalibrationTime; import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis; import edu.wpi.first.wpilibj.SPI.Port; import edu.wpi.first.wpilibj2.command.Command; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.Optional; import java.util.function.Supplier; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; import org.team340.lib.swerve.SwerveAPI; import org.team340.lib.swerve.SwerveAPI.ForwardPerspective; import org.team340.lib.swerve.config.SwerveConfig; @@ -18,6 +38,8 @@ import org.team340.lib.util.GRRSubsystem; import org.team340.robot.Constants; import org.team340.robot.Constants.RobotMap; +import org.team340.robot.tagslam.GtsamInterface; +import org.team340.robot.tagslam.TagDetection; @Logged public class Swerve extends GRRSubsystem { @@ -50,7 +72,7 @@ public class Swerve extends GRRSubsystem { .setTurnMotor(SwerveMotors.sparkFlex(RobotMap.BR_TURN, MotorType.kBrushless, true)) .setEncoder(SwerveEncoders.sparkFlexEncoder(0.6675, true)); - public static final SwerveConfig CONFIG = new SwerveConfig() + private static final SwerveConfig CONFIG = new SwerveConfig() .setTimings(Constants.PERIOD, Constants.PERIOD, 0.04) .setMovePID(0.0015, 0.0, 0.0, 0.0) .setMoveFF(0.05, 0.212) @@ -60,20 +82,113 @@ public class Swerve extends GRRSubsystem { .setDriverProfile(4.3, 1.0, 4.2, 2.0) .setPowerProperties(Constants.VOLTAGE, 80.0, 60.0) .setMechanicalProperties(6.75, 150.0 / 7.0, 0.0, Units.inchesToMeters(4.0)) - .setOdometryStd(0.003, 0.003, 0.0012) + .setOdometryStd(0.1, 0.1, 0.05) .setIMU(SwerveIMUs.adis16470(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, Port.kOnboardCS0, CalibrationTime._4s)) .setModules(FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT); + private static final Transform3d BACK_LEFT_CAMERA = new Transform3d( + new Translation3d(-0.29153, 0.26629, 0.24511), + new Rotation3d(0.0, Math.toRadians(-20.0), Math.toRadians(-160.0)) + ); + + private static final Transform3d BACK_RIGHT_CAMERA = new Transform3d( + new Translation3d(-0.29153, -0.26629, 0.24511), + new Rotation3d(0.0, Math.toRadians(-20.0), Math.toRadians(160.0)) + ); + private final SwerveAPI api; + private final AprilTagFieldLayout aprilTags; + private final GtsamInterface gtsamInterface; + private final PhotonCamera[] cameras; + private final PhotonPoseEstimator[] poseEstimators; + private final PhotonPipelineResult[] lastResults; + + private final List measurements = new ArrayList<>(); + private final List targets = new ArrayList<>(); + private Pose2d gtsamPose = new Pose2d(); + public Swerve() { api = new SwerveAPI(CONFIG); api.enableTunables("Swerve"); + + aprilTags = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + aprilTags.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + gtsamInterface = new GtsamInterface(List.of("backleft", "backright")); + cameras = new PhotonCamera[] { new PhotonCamera("backleft"), new PhotonCamera("backright") }; + poseEstimators = new PhotonPoseEstimator[] { + new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameras[0], BACK_LEFT_CAMERA), + new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameras[1], BACK_RIGHT_CAMERA) + }; + lastResults = new PhotonPipelineResult[cameras.length]; + Arrays.fill(lastResults, new PhotonPipelineResult()); } @Override public void periodic() { + long loopStart = WPIUtilJNI.now(); + api.refresh(); + + Twist2d twist = api.state.twist; + gtsamInterface.sendOdomUpdate(loopStart, new Twist3d(twist.dx, twist.dy, 0, 0, 0, twist.dtheta), null); + + measurements.clear(); + targets.clear(); + + for (int i = 0; i < cameras.length; i++) { + var camera = cameras[i]; + var poseEstimator = poseEstimators[i]; + var results = camera.getLatestResult(); + var last = lastResults[i]; + + List dets = new ArrayList<>(); + long tagDetTime = 0; + if (results.getTimestampSeconds() != last.getTimestampSeconds()) { + lastResults[i] = results; + for (var result : results.getTargets()) { + dets.add(new TagDetection(result.getFiducialId(), result.getDetectedCorners())); + } + + tagDetTime = loopStart - 10000; // YIPPEE + + var estimate = poseEstimators[i].update(results, camera.getCameraMatrix(), camera.getDistCoeffs()); + if (estimate.isPresent()) { + double sum = 0.0; + for (var target : estimate.get().targetsUsed) { + Optional tagPose = aprilTags.getTagPose(target.getFiducialId()); + if (tagPose.isEmpty()) continue; + targets.add(tagPose.get()); + sum += + api.state.pose.getTranslation().getDistance(tagPose.get().getTranslation().toTranslation2d()); + } + + int tagCount = estimate.get().targetsUsed.size(); + double stdScale = Math.pow(sum / tagCount, 2.0) / tagCount; + double xyStd = 0.1 * stdScale; + double angStd = 0.15 * stdScale; + + Pose2d estimatedPose = estimate.get().estimatedPose.toPose2d(); + api.addVisionMeasurement( + estimatedPose, + estimate.get().timestampSeconds, + VecBuilder.fill(xyStd, xyStd, angStd) + ); + measurements.add(estimatedPose); + continue; + } + } + + gtsamInterface.setCamIntrinsics(camera.getName(), camera.getCameraMatrix(), camera.getDistCoeffs()); + gtsamInterface.sendVisionUpdate( + camera.getName(), + tagDetTime, + dets, + poseEstimator.getRobotToCameraTransform() + ); + } + + gtsamPose = gtsamInterface.getLatencyCompensatedPoseEstimate().toPose2d(); } /** diff --git a/src/main/java/org/team340/robot/tagslam/GtsamInterface.java b/src/main/java/org/team340/robot/tagslam/GtsamInterface.java new file mode 100644 index 0000000..2c80a85 --- /dev/null +++ b/src/main/java/org/team340/robot/tagslam/GtsamInterface.java @@ -0,0 +1,231 @@ +package org.team340.robot.tagslam; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Twist3d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N5; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.networktables.StructSubscriber; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Optional; +import java.util.stream.Collectors; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; +import org.opencv.core.TermCriteria; +import org.photonvision.estimation.OpenCVHelp; +import org.photonvision.targeting.TargetCorner; + +public class GtsamInterface { + + public static class CameraCalibration { + + Mat cameraMat; + Mat distCoeffs; + + public CameraCalibration(Matrix camMat, Matrix distCoeffs) { + OpenCVHelp.forceLoadOpenCV(); + + this.cameraMat = OpenCVHelp.matrixToMat(camMat.getStorage()); + this.distCoeffs = OpenCVHelp.matrixToMat(distCoeffs.getStorage()); + } + + public void release() { + cameraMat.release(); + distCoeffs.release(); + } + } + + private static class CameraInterface { + + StructArrayPublisher tagPub; + DoubleArrayPublisher camIntrinsicsPublisher; + StructPublisher robotTcamPub; + private String name; + + private CameraCalibration cameraCal = null; + + public TagDetection undistort(TagDetection distorted) { + if (this.cameraCal == null) { + System.err.println("Camera cal still null -- is your camera connected?"); + return distorted; + } + + var mat = new MatOfPoint2f( + distorted.corners + .stream() + .map(it -> new Point(it.x, it.y)) + .collect(Collectors.toList()) + .toArray(new Point[0]) + ); + Calib3d.undistortImagePoints( + mat, + mat, + cameraCal.cameraMat, + cameraCal.distCoeffs, + new TermCriteria(3, 30, 1e-6) + ); + return new TagDetection( + distorted.id, + mat.toList().stream().map(it -> new TargetCorner(it.x, it.y)).collect(Collectors.toList()) + ); + } + + public CameraInterface(String name) { + this.name = name; + tagPub = NetworkTableInstance.getDefault() + .getStructArrayTopic("/gtsam_meme/" + name + "/input/tags", TagDetection.struct) + .publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true)); + robotTcamPub = NetworkTableInstance.getDefault() + .getStructTopic("/gtsam_meme/" + name + "/input/robotTcam", Transform3d.struct) + .publish(PubSubOption.sendAll(false), PubSubOption.keepDuplicates(false)); + camIntrinsicsPublisher = NetworkTableInstance.getDefault() + .getDoubleArrayTopic("/gtsam_meme/" + name + "/input/cam_intrinsics") + .publish(PubSubOption.sendAll(false), PubSubOption.keepDuplicates(false)); + } + } + + StructPublisher odomPub; + StructPublisher guessPub; + Map cameras = new HashMap<>(); + StructSubscriber optimizedPoseSub; + + // Estimated odom-only location relative to robot boot. We assume zero slip here + Pose3d localOdometryPose = new Pose3d(); + TimeInterpolatableBuffer odometryBuffer = TimeInterpolatableBuffer.createBuffer(5); + + public GtsamInterface(List cameraNames) { + odomPub = NetworkTableInstance.getDefault() + .getStructTopic("/gtsam_meme/input/odom_twist", Twist3d.struct) + .publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true)); + guessPub = NetworkTableInstance.getDefault() + .getStructTopic("/gtsam_meme/input/pose_initial_guess", Pose3d.struct) + .publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true)); + optimizedPoseSub = NetworkTableInstance.getDefault() + .getStructTopic("/gtsam_meme/output/optimized_pose", Pose3d.struct) + .subscribe(null, PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true)); + + cameraNames.stream().map(CameraInterface::new).forEach(it -> cameras.put(it.name, it)); + } + + /** + * Update the core camera intrinsic parameters. The localizer will apply these + * as soon as reasonable, and makes no attempt to latency compensate this. + * + * @param camName The name of the camera + * @param intrinsics Camera intrinsics in standard OpenCV format. See: + * https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html + * @param distCoeffs Camera distortion coefficients, of length 4, 5 or 8 + */ + public void setCamIntrinsics( + String camName, + Optional> intrinsics, + Optional> distCoeffs + ) { + if (intrinsics.isEmpty() || distCoeffs.isEmpty()) { + return; + } + + var cam = cameras.get(camName); + if (cam == null) { + throw new RuntimeException("Camera " + camName + " not in map!"); + } + + cam.camIntrinsicsPublisher.set( + new double[] { + intrinsics.get().get(0, 0), + intrinsics.get().get(1, 1), + intrinsics.get().get(0, 2), + intrinsics.get().get(1, 2) + } + ); + + cam.cameraCal = new CameraCalibration(intrinsics.get(), distCoeffs.get()); + } + + /** + * Update the localizer with new info from this robot loop iteration. + * + * @param odomTime The time that the odometry twist from last iteration + * was collected at, in microseconds. WPIUtilJNI::now is + * what I used + * @param odom The twist encoding chassis movement from last + * timestamp to now. I use + * SwerveDriveKinematics::toTwist2d + * @param guess An (optional, possibly null) initial guess at robot + * pose from solvePNP or prior knowledge. + */ + public void sendOdomUpdate(long odomTime, Twist3d odom, Pose3d guess) { + odomPub.set(odom, odomTime); + + if (guess != null) { + guessPub.set(guess, odomTime); + } + + localOdometryPose = localOdometryPose.exp(odom); + odometryBuffer.addSample(odomTime / 1e6, localOdometryPose); + } + + /** + * Update the localizer with new info from this robot loop iteration. + * + * @param camName The name of the camera + * @param tagDetTime The time that the frame encoding detected tags + * collected at, in microseconds. WPIUtilJNI::now is what + * I used + * @param camDetectedTags The list of tags this camera could see + * @param robotTcam Transform from robot to camera optical focal point. + * This is not latency compensated yet, so maybe don't + * put your camera on a turret + */ + public void sendVisionUpdate( + String camName, + long tagDetTime, + List camDetectedTags, + Transform3d robotTcam + ) { + var cam = cameras.get(camName); + if (cam == null) { + throw new RuntimeException("Camera " + camName + " not in map!"); + } + + cam.tagPub.set( + camDetectedTags + .stream() + .map(it -> cam.undistort(it)) + .collect(Collectors.toList()) + .toArray(new TagDetection[0]), + tagDetTime + ); + cam.robotTcamPub.set(robotTcam, tagDetTime); + } + + public Pose3d getLatencyCompensatedPoseEstimate() { + var poseEst = optimizedPoseSub.getAtomic(); + if (poseEst.timestamp != 0) { + var poseAtSample = odometryBuffer.getSample(poseEst.timestamp / 1e6); + var poseNow = localOdometryPose; + + if (poseAtSample.isEmpty()) { + // huh + return new Pose3d(); + } + + var poseDelta = poseNow.minus(poseAtSample.get()); + return poseEst.value.transformBy(poseDelta); + } else { + return new Pose3d(); + } + } +} diff --git a/src/main/java/org/team340/robot/tagslam/TagDetection.java b/src/main/java/org/team340/robot/tagslam/TagDetection.java new file mode 100644 index 0000000..33b3b20 --- /dev/null +++ b/src/main/java/org/team340/robot/tagslam/TagDetection.java @@ -0,0 +1,17 @@ +package org.team340.robot.tagslam; + +import java.util.List; +import org.photonvision.targeting.TargetCorner; + +public class TagDetection { + + public final int id; + public final List corners; + + public TagDetection(int id, List corners) { + this.id = id; + this.corners = corners; + } + + public static final TagDetectionStruct struct = new TagDetectionStruct(); +} diff --git a/src/main/java/org/team340/robot/tagslam/TagDetectionStruct.java b/src/main/java/org/team340/robot/tagslam/TagDetectionStruct.java new file mode 100644 index 0000000..f49ca99 --- /dev/null +++ b/src/main/java/org/team340/robot/tagslam/TagDetectionStruct.java @@ -0,0 +1,51 @@ +package org.team340.robot.tagslam; + +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; +import java.util.List; +import org.photonvision.targeting.TargetCorner; + +public class TagDetectionStruct implements Struct { + + @Override + public Class getTypeClass() { + return TagDetection.class; + } + + @Override + public String getTypeString() { + return "struct:TagDetection"; + } + + @Override + public int getSize() { + return ((8 * 2) * 4 + 4); + } + + @Override + public String getSchema() { + return "uint32 id;double cx1;double cy1;double cx2;double cy2;double " + "cx3;double cy3;double cx4;double cy4"; + } + + @Override + public TagDetection unpack(ByteBuffer bb) { + return new TagDetection( + bb.getInt(), + List.of( + new TargetCorner(bb.getDouble(), bb.getDouble()), + new TargetCorner(bb.getDouble(), bb.getDouble()), + new TargetCorner(bb.getDouble(), bb.getDouble()), + new TargetCorner(bb.getDouble(), bb.getDouble()) + ) + ); + } + + @Override + public void pack(ByteBuffer bb, TagDetection value) { + bb.putInt(value.id); + for (int i = 0; i < 4; i++) { + bb.putDouble(value.corners.get(i).x); + bb.putDouble(value.corners.get(i).y); + } + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..8f8e893 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.3.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2024.3.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.3.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2024.3.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.3.1" + } + ] +} \ No newline at end of file