-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #18 from chopshop-166/vision-lib
Vision lib
- Loading branch information
Showing
8 changed files
with
444 additions
and
149 deletions.
There are no files selected for viewing
41 changes: 41 additions & 0 deletions
41
src/main/java/com/chopshop166/chopshoplib/maps/CameraSource.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,41 @@ | ||
| package com.chopshop166.chopshoplib.maps; | ||
|
|
||
| import org.photonvision.PhotonCamera; | ||
| import org.photonvision.PhotonPoseEstimator; | ||
| import org.photonvision.PhotonPoseEstimator.PoseStrategy; | ||
| import edu.wpi.first.apriltag.AprilTagFieldLayout; | ||
| import edu.wpi.first.apriltag.AprilTagFields; | ||
| import edu.wpi.first.math.geometry.Transform3d; | ||
|
|
||
| /** Simplified wrapper around a camera with a pose estimator. */ | ||
| public class CameraSource { | ||
| /** The default field, loaded for convenience. */ | ||
| public static final AprilTagFieldLayout DEFAULT_FIELD = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); | ||
| /** The camera object. */ | ||
| public final PhotonCamera camera; | ||
| /** The pose estimator. */ | ||
| public final PhotonPoseEstimator estimator; | ||
|
|
||
| /** | ||
| * Constructor. | ||
| * | ||
| * @param cameraName The camera's name in Photon Vision. | ||
| * @param robotToCam The offset of the camera from the center of the robot. | ||
| */ | ||
| public CameraSource(final String cameraName, final Transform3d robotToCam) { | ||
| this(new PhotonCamera(cameraName), robotToCam); | ||
| } | ||
|
|
||
| /** | ||
| * Constructor. | ||
| * | ||
| * @param cameraName The camera object. | ||
| * @param robotToCam The offset of the camera from the center of the robot. | ||
| */ | ||
| public CameraSource(final PhotonCamera camera, final Transform3d robotToCam) { | ||
| this.camera = camera; | ||
| this.estimator = new PhotonPoseEstimator(DEFAULT_FIELD, | ||
| PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCam); | ||
| this.estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); | ||
| } | ||
| } |
54 changes: 54 additions & 0 deletions
54
src/main/java/com/chopshop166/chopshoplib/maps/VisionMap.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,54 @@ | ||
| package com.chopshop166.chopshoplib.maps; | ||
|
|
||
| import java.util.ArrayList; | ||
| import java.util.Arrays; | ||
| import java.util.List; | ||
| import edu.wpi.first.math.estimator.PoseEstimator; | ||
|
|
||
| public class VisionMap { | ||
|
|
||
| /** Camera-based position estimators. */ | ||
| public final List<CameraSource> visionSources; | ||
|
|
||
| /** Default constructor. */ | ||
| public VisionMap() { | ||
| this(new ArrayList<>()); | ||
| } | ||
|
|
||
| /** | ||
| * Constructor. | ||
| * | ||
| * @param visionSources Any number of vision sources. | ||
| */ | ||
| public VisionMap(final CameraSource... visionSources) { | ||
| this(Arrays.asList(visionSources)); | ||
| } | ||
|
|
||
| /** | ||
| * Constructor. | ||
| * | ||
| * @param visionSources A list of vision sources. | ||
| */ | ||
| public VisionMap(final List<CameraSource> visionSources) { | ||
| this.visionSources = visionSources; | ||
| } | ||
|
|
||
| /** | ||
| * Update the data in a pose estimator with the poses from all cameras. | ||
| * | ||
| * @param <T> Estimator wheel type. | ||
| * @param estimator The WPIlib estimator object. | ||
| */ | ||
| public <T> void updateData(final PoseEstimator<T> estimator) { | ||
| for (var source : this.visionSources) { | ||
| var results = source.camera.getAllUnreadResults(); | ||
| if (!results.isEmpty()) { | ||
| var estimate = source.estimator.update(results.get(results.size() - 1)); | ||
| estimate.ifPresent(est -> { | ||
| estimator.addVisionMeasurement(est.estimatedPose.toPose2d(), | ||
| est.timestampSeconds); | ||
| }); | ||
| } | ||
| } | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,170 @@ | ||
| package frc.robot; | ||
|
|
||
| // Copyright (c) 2025 FRC 6328 | ||
| // http://github.com/Mechanical-Advantage | ||
| // | ||
| // Use of this source code is governed by an MIT-style | ||
| // license that can be found in the LICENSE file at | ||
| // the root directory of this project. | ||
|
|
||
| import edu.wpi.first.math.geometry.*; | ||
| import edu.wpi.first.math.util.Units; | ||
| import java.util.ArrayList; | ||
| import java.util.HashMap; | ||
| import java.util.List; | ||
| import java.util.Map; | ||
|
|
||
| /** | ||
| * Contains various field dimensions and useful reference points. All units are | ||
| * in meters and poses | ||
| * have a blue alliance origin. | ||
| */ | ||
| public class FieldConstants { | ||
| public static final double fieldLength = Units.inchesToMeters(690.876); | ||
| public static final double fieldWidth = Units.inchesToMeters(317); | ||
| public static final double startingLineX = Units.inchesToMeters(299.438); // Measured from the inside of starting | ||
| // line | ||
|
|
||
| public static class Processor { | ||
| public static final Pose2d centerFace = new Pose2d(Units.inchesToMeters(235.726), 0, | ||
| Rotation2d.fromDegrees(90)); | ||
| } | ||
|
|
||
| public static class Barge { | ||
| public static final Translation2d farCage = new Translation2d(Units.inchesToMeters(345.428), | ||
| Units.inchesToMeters(286.779)); | ||
| public static final Translation2d middleCage = new Translation2d(Units.inchesToMeters(345.428), | ||
| Units.inchesToMeters(242.855)); | ||
| public static final Translation2d closeCage = new Translation2d(Units.inchesToMeters(345.428), | ||
| Units.inchesToMeters(199.947)); | ||
|
|
||
| // Measured from floor to bottom of cage | ||
| public static final double deepHeight = Units.inchesToMeters(3.125); | ||
| public static final double shallowHeight = Units.inchesToMeters(30.125); | ||
| } | ||
|
|
||
| public static class CoralStation { | ||
| public static final Pose2d leftCenterFace = new Pose2d( | ||
| Units.inchesToMeters(33.526), | ||
| Units.inchesToMeters(291.176), | ||
| Rotation2d.fromDegrees(90 - 144.011)); | ||
| public static final Pose2d rightCenterFace = new Pose2d( | ||
| Units.inchesToMeters(33.526), | ||
| Units.inchesToMeters(25.824), | ||
| Rotation2d.fromDegrees(144.011 - 90)); | ||
| } | ||
|
|
||
| public static class Reef { | ||
| public static final Translation2d center = new Translation2d(Units.inchesToMeters(176.746), | ||
| Units.inchesToMeters(158.501)); | ||
| public static final double faceToZoneLine = Units.inchesToMeters(12); // Side of the reef to the inside of the | ||
| // reef zone line | ||
|
|
||
| public static final Pose2d[] centerFaces = new Pose2d[6]; // Starting facing the driver station in clockwise | ||
| // order | ||
| public static final List<Map<ReefHeight, Pose3d>> branchPositions = new ArrayList<>(); // Starting at the right | ||
| // branch facing the | ||
| // driver station in | ||
| // clockwise | ||
|
|
||
| static { | ||
| // Initialize faces | ||
| centerFaces[0] = new Pose2d( | ||
| Units.inchesToMeters(144.003), | ||
| Units.inchesToMeters(158.500), | ||
| Rotation2d.fromDegrees(180)); | ||
| centerFaces[1] = new Pose2d( | ||
| Units.inchesToMeters(160.373), | ||
| Units.inchesToMeters(186.857), | ||
| Rotation2d.fromDegrees(120)); | ||
| centerFaces[2] = new Pose2d( | ||
| Units.inchesToMeters(193.116), | ||
| Units.inchesToMeters(186.858), | ||
| Rotation2d.fromDegrees(60)); | ||
| centerFaces[3] = new Pose2d( | ||
| Units.inchesToMeters(209.489), | ||
| Units.inchesToMeters(158.502), | ||
| Rotation2d.fromDegrees(0)); | ||
| centerFaces[4] = new Pose2d( | ||
| Units.inchesToMeters(193.118), | ||
| Units.inchesToMeters(130.145), | ||
| Rotation2d.fromDegrees(-60)); | ||
| centerFaces[5] = new Pose2d( | ||
| Units.inchesToMeters(160.375), | ||
| Units.inchesToMeters(130.144), | ||
| Rotation2d.fromDegrees(-120)); | ||
|
|
||
| // Initialize branch positions | ||
| for (int face = 0; face < 6; face++) { | ||
| Map<ReefHeight, Pose3d> fillRight = new HashMap<>(); | ||
| Map<ReefHeight, Pose3d> fillLeft = new HashMap<>(); | ||
| for (var level : ReefHeight.values()) { | ||
| Pose2d poseDirection = new Pose2d(center, Rotation2d.fromDegrees(180 - (60 * face))); | ||
| double adjustX = Units.inchesToMeters(30.738); | ||
| double adjustY = Units.inchesToMeters(6.469); | ||
|
|
||
| fillRight.put( | ||
| level, | ||
| new Pose3d( | ||
| new Translation3d( | ||
| poseDirection | ||
| .transformBy(new Transform2d(adjustX, adjustY, new Rotation2d())) | ||
| .getX(), | ||
| poseDirection | ||
| .transformBy(new Transform2d(adjustX, adjustY, new Rotation2d())) | ||
| .getY(), | ||
| level.height), | ||
| new Rotation3d( | ||
| 0, | ||
| Units.degreesToRadians(level.pitch), | ||
| poseDirection.getRotation().getRadians()))); | ||
| fillLeft.put( | ||
| level, | ||
| new Pose3d( | ||
| new Translation3d( | ||
| poseDirection | ||
| .transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d())) | ||
| .getX(), | ||
| poseDirection | ||
| .transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d())) | ||
| .getY(), | ||
| level.height), | ||
| new Rotation3d( | ||
| 0, | ||
| Units.degreesToRadians(level.pitch), | ||
| poseDirection.getRotation().getRadians()))); | ||
| } | ||
| branchPositions.add((face * 2) + 1, fillRight); | ||
| branchPositions.add((face * 2) + 2, fillLeft); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| public static class StagingPositions { | ||
| // Measured from the center of the ice cream (coral marks) | ||
| public static final Pose2d leftIceCream = new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(230.5), | ||
| new Rotation2d()); | ||
| public static final Pose2d middleIceCream = new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), | ||
| new Rotation2d()); | ||
| public static final Pose2d rightIceCream = new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), | ||
| new Rotation2d()); | ||
| } | ||
|
|
||
| public enum ReefHeight { | ||
| L4(Units.inchesToMeters(72), -90), | ||
| L3(Units.inchesToMeters(47.625), -35), | ||
| L2(Units.inchesToMeters(31.875), -35), | ||
| L1(Units.inchesToMeters(18), 0); | ||
|
|
||
| ReefHeight(double height, double pitch) { | ||
| this.height = height; | ||
| this.pitch = pitch; // in degrees | ||
| } | ||
|
|
||
| public final double height; | ||
| public final double pitch; | ||
| } | ||
|
|
||
| public static final double aprilTagWidth = Units.inchesToMeters(6.50); | ||
| public static final int aprilTagCount = 22; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.