From ddc04e079cba49e2817a35376e6239576d1dba13 Mon Sep 17 00:00:00 2001 From: sleepyghost <145573597+sleepyghost-zzz@users.noreply.github.com> Date: Mon, 9 Dec 2024 10:33:47 -0500 Subject: [PATCH] Add javadoc for vision and parameter tools (#68) * Added Javadocs for all of vision. * Added Javadocs for all of parameter_tools --- .../parameter_tools/JSONExclude.java | 18 +++ .../coppercore/parameter_tools/JSONSync.java | 141 ++++++++++++++---- .../src/test/java/ExampleJsonSyncClass.java | 32 +++- .../src/test/java/JSONSyncTests.java | 49 ++++-- .../main/java/coppercore/vision/Camera.java | 44 +++++- .../coppercore/vision/CameraContainer.java | 14 +- .../vision/CameraContainerReal.java | 22 +++ .../vision/CameraContainerReplay.java | 22 ++- .../coppercore/vision/CameraContainerSim.java | 50 ++++--- .../main/java/coppercore/vision/CameraIO.java | 31 +++- .../coppercore/vision/CameraIOPhoton.java | 73 +++++++-- .../java/coppercore/vision/CameraParams.java | 10 ++ .../vision/CoreVisionConstants.java | 2 +- .../java/coppercore/vision/LayoutUtil.java | 9 +- .../coppercore/vision/VisionLocalizer.java | 27 +++- 15 files changed, 459 insertions(+), 85 deletions(-) diff --git a/parameter_tools/src/main/java/coppercore/parameter_tools/JSONExclude.java b/parameter_tools/src/main/java/coppercore/parameter_tools/JSONExclude.java index 894e071..b195501 100644 --- a/parameter_tools/src/main/java/coppercore/parameter_tools/JSONExclude.java +++ b/parameter_tools/src/main/java/coppercore/parameter_tools/JSONExclude.java @@ -5,6 +5,24 @@ import java.lang.annotation.RetentionPolicy; import java.lang.annotation.Target; +/** + * Annotation to exclude fields from being serialized or deserialized in JSON. + * + *

When applied to a field, the field will be ignored during both the serialization and + * deserialization processes. Useful for marking fields that should not be part of the JSON + * representation. + * + *

Usage example: + * + * 

{@code
+ * public class Example {
+ *     private String includedField;
+ *
+ *     @JSONExclude
+ *     private String excludedField;
+ * }
+ * }
+ */ @Retention(RetentionPolicy.RUNTIME) @Target(ElementType.FIELD) public @interface JSONExclude {} diff --git a/parameter_tools/src/main/java/coppercore/parameter_tools/JSONSync.java b/parameter_tools/src/main/java/coppercore/parameter_tools/JSONSync.java index 7dbb70b..8664f2b 100644 --- a/parameter_tools/src/main/java/coppercore/parameter_tools/JSONSync.java +++ b/parameter_tools/src/main/java/coppercore/parameter_tools/JSONSync.java @@ -11,44 +11,43 @@ import java.io.FileWriter; import java.io.IOException; +/** + * A utility class for synchronizing Java objects with JSON files. Provides functionality for + * loading and saving data to a JSON file using the Gson library. + */ public class JSONSync { - private final Gson gson; - - private T instance; - - private String file; - - private final JSONSyncConfig config; + private final Gson gson; // Gson instance for serialization and deserialization + private T instance; // The object being synchronized + private String file; // File path for the JSON file + private final JSONSyncConfig config; // Configuration for Gson + /** + * Retrieves the object synchronized with the JSON file. + * + * @return The synchronized object. + */ public T getObject() { return instance; } - private FileReader getFileReader(String path) { - try { - return new FileReader(path); - } catch (FileNotFoundException e) { - throw new RuntimeException("File not found " + path, e); - } - } - - private FileWriter getFileWriter(String path) { - try { - return new FileWriter(path); - } catch (IOException e) { - throw new RuntimeException("IOException " + path, e); - } - } - + /** + * Loads the data from the JSON file into the object. + * + * @throws RuntimeException if the JSON file cannot be read or parsed. + */ @SuppressWarnings("unchecked") public void loadData() { instance = gson.fromJson(getFileReader(file), (Class) instance.getClass()); } + /** + * Saves the current state of the object to the JSON file. + * + * @throws RuntimeException if the JSON file cannot be written. + */ public void saveData() { String json = gson.toJson(instance); - // System.out.println(json); FileWriter writer = getFileWriter(file); try { writer.write(json); @@ -58,10 +57,34 @@ public void saveData() { } } + /** + * Sets the file path for the JSON file. + * + * @param newFilePath The new file path for the JSON file. + */ public void setFile(String newFilePath) { file = newFilePath; } + /** + * Constructs a new JSONSync instance. + * + * @param instance The object to synchronize with the JSON file. + * @param file The file path for the JSON file. + * @param config Configuration for the Gson instance; uses defaults if null. + */ + public JSONSync(T instance, String file, JSONSyncConfig config) { + this.instance = instance; + this.config = (config == null) ? new JSONSyncConfigBuilder().build() : config; + this.gson = generateGson(); + this.file = file; + } + + /** + * Generates a Gson instance with the current configuration. + * + * @return A configured Gson instance. + */ private Gson generateGson() { ExclusionStrategy strategy = new ExclusionStrategy() { @@ -88,13 +111,37 @@ public boolean shouldSkipClass(Class clazz) { return builder.create(); } - public JSONSync(T instance, String file, JSONSyncConfig config) { - this.instance = instance; - this.config = (config == null) ? new JSONSyncConfigBuilder().build() : config; - this.gson = generateGson(); - this.file = file; + /** + * Provides a FileReader for the given file path. + * + * @param path The file path. + * @return A FileReader for the specified file. + * @throws RuntimeException if the file cannot be found. + */ + private FileReader getFileReader(String path) { + try { + return new FileReader(path); + } catch (FileNotFoundException e) { + throw new RuntimeException("File not found " + path, e); + } + } + + /** + * Provides a FileWriter for the given file path. + * + * @param path The file path. + * @return A FileWriter for the specified file. + * @throws RuntimeException if the file cannot be created or written to. + */ + private FileWriter getFileWriter(String path) { + try { + return new FileWriter(path); + } catch (IOException e) { + throw new RuntimeException("IOException " + path, e); + } } + /** Configuration class for customizing Gson behavior. */ private static record JSONSyncConfig( boolean serializeNulls, boolean prettyPrinting, @@ -111,6 +158,7 @@ public JSONSyncConfig(JSONSyncConfigBuilder builder) { } } + /** Builder class for creating a JSONSyncConfig instance. */ public static class JSONSyncConfigBuilder { public boolean serializeNulls = false; public boolean prettyPrinting = false; @@ -118,33 +166,68 @@ public static class JSONSyncConfigBuilder { public FieldNamingPolicy namingPolicy = FieldNamingPolicy.IDENTITY; public LongSerializationPolicy longSerializationPolicy = LongSerializationPolicy.DEFAULT; + /** + * Sets whether null fields should be serialized. + * + * @param serializeNulls True to serialize null fields, false otherwise. + * @return The builder instance. + */ public JSONSyncConfigBuilder setSerializeNulls(boolean serializeNulls) { this.serializeNulls = serializeNulls; return this; } + /** + * Sets whether the JSON output should use pretty printing. + * + * @param prettyPrinting True to enable pretty printing, false otherwise. + * @return The builder instance. + */ public JSONSyncConfigBuilder setPrettyPrinting(boolean prettyPrinting) { this.prettyPrinting = prettyPrinting; return this; } + /** + * Sets whether fields without @Expose annotations should be excluded. + * + * @param excludeFieldsWithoutExposeAnnotation True to exclude fields, false otherwise. + * @return The builder instance. + */ public JSONSyncConfigBuilder setExcludeFieldsWithoutExposeAnnotation( boolean excludeFieldsWithoutExposeAnnotation) { this.excludeFieldsWithoutExposeAnnotation = excludeFieldsWithoutExposeAnnotation; return this; } + /** + * Sets the naming policy for fields in the JSON output. + * + * @param namingPolicy The field naming policy. + * @return The builder instance. + */ public JSONSyncConfigBuilder setNamingPolicy(FieldNamingPolicy namingPolicy) { this.namingPolicy = namingPolicy; return this; } + /** + * Sets the serialization policy for long values. + * + * @param longSerializationPolicy The serialization policy for long values. + * @return The builder instance. + */ public JSONSyncConfigBuilder setLongSerializationPolicy( LongSerializationPolicy longSerializationPolicy) { this.longSerializationPolicy = longSerializationPolicy; return this; } + /** + * Builds the configuration object. + * + * @return A JSONSyncConfig instance. + */ public JSONSyncConfig build() { return new JSONSyncConfig(this); } diff --git a/parameter_tools/src/test/java/ExampleJsonSyncClass.java b/parameter_tools/src/test/java/ExampleJsonSyncClass.java index f7f4655..60d591f 100644 --- a/parameter_tools/src/test/java/ExampleJsonSyncClass.java +++ b/parameter_tools/src/test/java/ExampleJsonSyncClass.java @@ -2,24 +2,53 @@ import coppercore.parameter_tools.JSONSync; +/** + * Example class to demonstrate the usage of the {@link JSONSync} utility for saving and loading + * data in JSON format. + * + *

This class showcases how fields can be serialized and deserialized, including nested objects, + * using the {@link JSONSync} functionality. + * + *

Key features: + * + *

+ */ public class ExampleJsonSyncClass { + /** + * Static {@link JSONSync} instance for managing the serialization and deserialization of this + * class. + */ public static JSONSync synced = - new JSONSync( + new JSONSync<>( new ExampleJsonSyncClass(), "filePath", new JSONSync.JSONSyncConfigBuilder().build()); public final String testText = ""; + public final Double testDouble = 0.0; + public Integer testInt = 0; + public final BasicMotorDataHolder motorData = null; + /** Nested class to represent motor-related data. */ public class BasicMotorDataHolder { + public final Double maxVoltage = 0.0; + public final Double minVoltage = 0.0; + public Double currentVoltage = 0.0; + @Override public String toString() { return "minVoltage: " + minVoltage @@ -30,6 +59,7 @@ public String toString() { } } + @Override public String toString() { return "testText: " + testText diff --git a/parameter_tools/src/test/java/JSONSyncTests.java b/parameter_tools/src/test/java/JSONSyncTests.java index fb328a2..d69ccef 100644 --- a/parameter_tools/src/test/java/JSONSyncTests.java +++ b/parameter_tools/src/test/java/JSONSyncTests.java @@ -6,50 +6,77 @@ import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; +/** + * Unit tests for the {@link JSONSync} class to validate its functionality, including data loading, + * file setting, and data saving. + * + *

This test class uses JUnit 5 to ensure that {@link JSONSync} correctly serializes and + * deserializes objects to and from JSON files. It also verifies that {@link JSONSync} updates files + * when data changes. + */ public class JSONSyncTests { + /** Directory paths for storing and accessing test JSON files. */ public static final String DIRECTORY = new File("").getAbsolutePath(); + public static final String BUILD_DIRECTORY = DIRECTORY + "/build"; public static final String RESOURCE_DIRECTORY = BUILD_DIRECTORY + "/resources/test"; + /** + * Prepares a new {@link JSONSync} instance with a predefined configuration and file path before + * each test. + */ @BeforeEach public void TestPrep() { ExampleJsonSyncClass.synced = - new JSONSync( + new JSONSync<>( new ExampleJsonSyncClass(), RESOURCE_DIRECTORY + "/ExampleJsonSyncClass.json", new JSONSync.JSONSyncConfigBuilder().setPrettyPrinting(true).build()); } + /** + * Tests the {@link JSONSync#loadData} method to ensure data is correctly loaded from a JSON + * file. + */ @Test public void JsonSyncLoadDataTest() { ExampleJsonSyncClass.synced.loadData(); ExampleJsonSyncClass instance = ExampleJsonSyncClass.synced.getObject(); - Assertions.assertEquals(10.0, instance.testDouble); - Assertions.assertEquals(2, instance.testInt); - Assertions.assertNull(instance.motorData); + Assertions.assertEquals(10.0, instance.testDouble, "testDouble should be 10.0"); + Assertions.assertEquals(2, instance.testInt, "testInt should be 2"); + Assertions.assertNull(instance.motorData, "motorData should be null"); } + /** + * Tests the {@link JSONSync#setFile} method to verify it properly updates the file path and + * loads data from the newly specified file. + */ @Test public void JsonSyncSetFileTest() { ExampleJsonSyncClass.synced.setFile(RESOURCE_DIRECTORY + "/SetFileTest.json"); ExampleJsonSyncClass.synced.loadData(); ExampleJsonSyncClass instance = ExampleJsonSyncClass.synced.getObject(); - Assertions.assertEquals(10.0, instance.testDouble); - Assertions.assertEquals(2, instance.testInt); - Assertions.assertNotNull(instance.motorData); - Assertions.assertEquals(-12.3, instance.motorData.minVoltage); - Assertions.assertEquals(16.4, instance.motorData.maxVoltage); - Assertions.assertEquals(0.0, instance.motorData.currentVoltage); + Assertions.assertEquals(10.0, instance.testDouble, "testDouble should be 10.0"); + Assertions.assertEquals(2, instance.testInt, "testInt should be 2"); + Assertions.assertNotNull(instance.motorData, "motorData should not be null"); + Assertions.assertEquals(-12.3, instance.motorData.minVoltage, "minVoltage should be -12.3"); + Assertions.assertEquals(16.4, instance.motorData.maxVoltage, "maxVoltage should be 16.4"); + Assertions.assertEquals( + 0.0, instance.motorData.currentVoltage, "currentVoltage should be 0.0"); } + /** + * Tests the {@link JSONSync#saveData} method to ensure changes to an object are serialized and + * saved to a JSON file correctly. + */ @Test public void JsonSyncSaveFileTest() { ExampleJsonSyncClass.synced.loadData(); ExampleJsonSyncClass instance = ExampleJsonSyncClass.synced.getObject(); - instance.testInt = 10; + instance.testInt = 10; // Modify a value in the object ExampleJsonSyncClass.synced.setFile(RESOURCE_DIRECTORY + "/SaveFileTest.json"); ExampleJsonSyncClass.synced.saveData(); } diff --git a/vision/src/main/java/coppercore/vision/Camera.java b/vision/src/main/java/coppercore/vision/Camera.java index 48c6597..ccd50c0 100644 --- a/vision/src/main/java/coppercore/vision/Camera.java +++ b/vision/src/main/java/coppercore/vision/Camera.java @@ -6,12 +6,25 @@ import edu.wpi.first.math.numbers.N3; import org.littletonrobotics.junction.Logger; +/** + * Represents a PhotonVision camera by wrapping a {@link CameraIO} interface. This class is + * responsible for managing the camera's IO, logging camera data, calculating standard deviations, + * assembling {@link CameraMeasurement} objects, and updating the camera's state. + */ public class Camera { + /** The name of the camera, typically defined in {@link CameraParams}. */ public final String name; private final CameraIO io; private final CameraIOInputsAutoLogged inputs; + /** + * Constructs a new {@code Camera} instance. + * + * @param params The parameters used to configure the camera, such as its name. + * @param io The {@link CameraIO} instance that provides the camera's input/output + * functionality. + */ public Camera(CameraParams params, CameraIO io) { name = params.name(); @@ -19,24 +32,53 @@ public Camera(CameraParams params, CameraIO io) { inputs = new CameraIOInputsAutoLogged(); } + /** + * Updates the camera's inputs by polling the {@link CameraIO} and processes the data for + * logging using the {@link Logger}. + */ public void update() { io.updateInputs(inputs); Logger.processInputs("Vision/" + name, inputs); } + /** + * Checks if the camera has new measurements available. + * + * @return {@code true} if new measurements are available, as indicated by {@link + * coppercore.vision.CameraIO.CameraIOInputs#wasAccepted}. + */ public boolean hasNewMeasurement() { return inputs.wasAccepted; } + /** + * Checks if the camera is currently connected. + * + * @return {@code true} if the camera is connected, as indicated by {@link + * coppercore.vision.CameraIO.CameraIOInputs#connected}. + */ public boolean isConnected() { return inputs.connected; } + /** + * Retrieves the latest measurement from the camera, including the field-to-robot + * transformation, timestamp, and variance. + * + * @return A {@link CameraMeasurement} object containing the latest data. + */ public CameraMeasurement getLatestMeasurement() { return new CameraMeasurement( inputs.latestFieldToRobot, inputs.latestTimestampSeconds, getLatestVariance()); } + /** + * Calculates the variance of the latest measurement based on the distance to the target and the + * number of detected tags. + * + * @return A {@link Matrix} representing the variance of the latest measurement, with dimensions + * {@code N3 x N1}. + */ public Matrix getLatestVariance() { Matrix stdDev = CoreVisionConstants.singleTagStdDev; // TODO: Actually calculate variances! @@ -52,7 +94,7 @@ public Matrix getLatestVariance() { return CoreVisionConstants.rejectionStdDev; } - // distance based variance + // Distance-based variance stdDev = stdDev.times( 1 diff --git a/vision/src/main/java/coppercore/vision/CameraContainer.java b/vision/src/main/java/coppercore/vision/CameraContainer.java index 40bbc78..e44f16b 100644 --- a/vision/src/main/java/coppercore/vision/CameraContainer.java +++ b/vision/src/main/java/coppercore/vision/CameraContainer.java @@ -2,10 +2,22 @@ import java.util.List; -/** This is not an AdvantageKit IO interface, but a class to hold cameras and associated data */ +/** + * Represents a container for managing multiple {@link Camera} objects and their associated data. + * This interface provides methods to retrieve the cameras and update their state. + * + *

Note: This is not an AdvantageKit IO interface but rather a utility for grouping and managing + * cameras. + */ public interface CameraContainer { + /** + * Retrieves the list of {@link Camera} objects managed by this container. + * + * @return A {@link List} of {@link Camera} objects. + */ public List getCameras(); + /** Updates the state of all {@link Camera} objects in the container. */ public void update(); } diff --git a/vision/src/main/java/coppercore/vision/CameraContainerReal.java b/vision/src/main/java/coppercore/vision/CameraContainerReal.java index fcf6bc5..3f48221 100644 --- a/vision/src/main/java/coppercore/vision/CameraContainerReal.java +++ b/vision/src/main/java/coppercore/vision/CameraContainerReal.java @@ -4,20 +4,42 @@ import java.util.ArrayList; import java.util.List; +/** + * A implementation of {@link CameraContainer} that manages a list of {@link Camera} objects for + * real-life matches. This class initializes cameras based on a list of {@link CameraParams} and an + * {@link AprilTagFieldLayout}, and provides methods to retrieve and update them. + */ public class CameraContainerReal implements CameraContainer { private List cameras = new ArrayList<>(); + /** + * Constructs a {@code CameraContainerReal} with a list of camera parameters and an AprilTag + * field layout. Each {@link Camera} is initialized with the given parameters and a + * corresponding {@link CameraIOPhoton}. + * + * @param params the list of {@link CameraParams} used to configure the cameras + * @param layout the {@link AprilTagFieldLayout} associated with the cameras + */ public CameraContainerReal(List params, AprilTagFieldLayout layout) { for (CameraParams param : params) { cameras.add(new Camera(param, CameraIOPhoton.fromRealCameraParams(param, layout))); } } + /** + * Retrieves the list of cameras managed by this container. + * + * @return a {@link List} of {@link Camera} objects + */ public List getCameras() { return cameras; } + /** + * Updates all cameras in the container by invoking their {@link Camera#update()} method. This + * ensures the cameras are synchronized with the latest data. + */ public void update() { for (Camera camera : cameras) { camera.update(); diff --git a/vision/src/main/java/coppercore/vision/CameraContainerReplay.java b/vision/src/main/java/coppercore/vision/CameraContainerReplay.java index 1d207df..ad0e9b8 100644 --- a/vision/src/main/java/coppercore/vision/CameraContainerReplay.java +++ b/vision/src/main/java/coppercore/vision/CameraContainerReplay.java @@ -3,19 +3,39 @@ import java.util.ArrayList; import java.util.List; +/** + * CameraContainerReplay simulates a container for cameras in a replay system. It does not interact + * with a real camera or simulation, and instead it updates the cameras based on replayed data. + */ public class CameraContainerReplay implements CameraContainer { private List cameras = new ArrayList<>(); + /** + * Initializes the CameraContainerReplay with the specified camera parameters. + * + * @param params A list of camera parameters for the replay. + */ public CameraContainerReplay(List params) { for (CameraParams param : params) { - cameras.add(new Camera(param, new CameraIO() {})); + cameras.add(new Camera(param, new CameraIO() {})); // Use a no-op CameraIO for replay } } + /** + * Gets the list of cameras in the replay system. + * + * @return A list of cameras. + */ + @Override public List getCameras() { return cameras; } + /** + * Updates the cameras by calling the update method on each camera. This does not interact with + * a real vision system but updates based on replayed data. + */ + @Override public void update() { for (Camera camera : cameras) { camera.update(); diff --git a/vision/src/main/java/coppercore/vision/CameraContainerSim.java b/vision/src/main/java/coppercore/vision/CameraContainerSim.java index dc215fc..1244f27 100644 --- a/vision/src/main/java/coppercore/vision/CameraContainerSim.java +++ b/vision/src/main/java/coppercore/vision/CameraContainerSim.java @@ -2,7 +2,6 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; 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.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -15,12 +14,14 @@ import org.littletonrobotics.junction.Logger; import org.photonvision.simulation.VisionSystemSim; +/** + * CameraContainerSim is used for handling cameras in a sim. It simulates the camera's interactions + * with the robot's state and the field's layout. + */ public class CameraContainerSim implements CameraContainer { private VisionSystemSim visionSim = new VisionSystemSim("main"); - private List cameras = new ArrayList<>(); - private Supplier getModuleStates; private Pose2d latestOdometryPose; private SwerveModulePosition[] lastModulePositions = @@ -31,9 +32,17 @@ public class CameraContainerSim implements CameraContainer { new SwerveModulePosition() }; private Timer dtTimer = new Timer(); - - SwerveDriveKinematics kinematics; - + private SwerveDriveKinematics kinematics; + + /** + * Initializes a CameraContainerSim with camera parameters and field layout. + * + * @param params A list of camera parameters for the simulation. + * @param layout The layout of the AprilTags in the simulation. + * @param initialPose The initial pose of the robot. + * @param kinematics The kinematics for the robot's swerve drive. + * @param getModuleStates A supplier for the current swerve module states. + */ public CameraContainerSim( List params, AprilTagFieldLayout layout, @@ -41,16 +50,10 @@ public CameraContainerSim( SwerveDriveKinematics kinematics, Supplier getModuleStates) { this.getModuleStates = getModuleStates; - visionSim.addAprilTags(layout); - this.kinematics = kinematics; - latestOdometryPose = - new Pose2d( - initialPose.getX(), - initialPose.getY(), - Rotation2d.fromRadians(initialPose.getRotation().getRadians())); + new Pose2d(initialPose.getX(), initialPose.getY(), initialPose.getRotation()); for (CameraParams param : params) { cameras.add( @@ -60,10 +63,21 @@ public CameraContainerSim( } } + /** + * Gets the list of cameras in the simulation. + * + * @return A list of cameras. + */ + @Override public List getCameras() { return cameras; } + /** + * Updates the odometry and camera states for the simulation. Updates both the simulated robot + * pose and camera measurements. + */ + @Override public void update() { updateOdometry(); visionSim.update(latestOdometryPose); @@ -74,24 +88,20 @@ public void update() { } } + /** Updates the robot's odometry based on the latest swerve module states. */ private void updateOdometry() { - SwerveModulePosition[] deltas = new SwerveModulePosition[4]; SwerveModuleState[] states = getModuleStates.get(); - double dt = dtTimer.get(); dtTimer.reset(); dtTimer.start(); + SwerveModulePosition[] deltas = new SwerveModulePosition[4]; for (int i = 0; i < states.length; i++) { deltas[i] = new SwerveModulePosition( states[i].speedMetersPerSecond * dt - lastModulePositions[i].distanceMeters, - Rotation2d.fromRadians( - states[i] - .angle - .minus(lastModulePositions[i].angle) - .getRadians())); + states[i].angle.minus(lastModulePositions[i].angle)); } Twist2d twist = kinematics.toTwist2d(deltas); diff --git a/vision/src/main/java/coppercore/vision/CameraIO.java b/vision/src/main/java/coppercore/vision/CameraIO.java index 5bb298d..41fef94 100644 --- a/vision/src/main/java/coppercore/vision/CameraIO.java +++ b/vision/src/main/java/coppercore/vision/CameraIO.java @@ -4,25 +4,52 @@ import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; +/** + * Interface representing the input/output (IO) functionality for a camera in the vision system. + * This interface provides a method to update camera inputs and includes an inner class for + * encapsulating camera-related data. + */ public interface CameraIO { + /** + * Class representing the inputs from a camera, including pose, tag information, and connection + * status. + */ @AutoLog public static class CameraIOInputs { + /** The most recent pose of the robot relative to the field, as calculated by the camera. */ public Pose3d latestFieldToRobot = new Pose3d(); + + /** The average distance to detected tags in meters. */ public double averageTagDistanceM = 0.0; + + /** The average yaw (rotation) of detected tags, represented as a {@link Rotation2d}. */ public Rotation2d averageTagYaw = new Rotation2d(); + + /** The number of tags currently detected by the camera. */ public int nTags = 0; + /** The timestamp of the latest measurement in seconds. */ public double latestTimestampSeconds = 0.0; + + /** Whether the camera is currently connected. */ public boolean connected = false; + + /** Indicates if a new measurement has been received from the camera. */ public boolean isNewMeasurement = false; /** - * Whether the new camera measurement was accepted by the initial filters. Always false if - * `isNewMeasurement` is false. + * Indicates whether the new camera measurement was accepted by the initial filters. This is + * always {@code false} if {@code isNewMeasurement} is {@code false}. */ public boolean wasAccepted = false; } + /** + * Updates the provided {@link CameraIOInputs} object with the latest camera data. This default + * implementation does nothing and should be overridden by implementing classes. + * + * @param inputs The {@link CameraIOInputs} object to update with the latest camera data. + */ public default void updateInputs(CameraIOInputs inputs) {} } diff --git a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java index a76c79f..84504e1 100644 --- a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java +++ b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java @@ -14,16 +14,42 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +/** + * An implementation of {@link CameraIO} that integrates with PhotonVision cameras for real-time + * pose estimation using AprilTags. This class handles camera updates, data simulation, and + * field-to-robot transformations. + * + *

This class supports both real-world and simulated cameras and provides utility methods for + * processing tag data such as distances and yaw angles. + */ public class CameraIOPhoton implements CameraIO { + private final PhotonCamera camera; private final PhotonPoseEstimator poseEstimator; - private double latestTimestampSeconds = 0.0; + /** + * Creates a new {@code CameraIOPhoton} instance using a camera name, field layout, and the + * robot-to-camera transform. + * + * @param name The name of the camera. + * @param layout The {@link AprilTagFieldLayout} describing the field's AprilTag configuration. + * @param robotToCamera The {@link Transform3d} representing the transformation from the robot + * to the camera. + */ public CameraIOPhoton(String name, AprilTagFieldLayout layout, Transform3d robotToCamera) { this(new PhotonCamera(name), layout, robotToCamera); } + /** + * Creates a new {@code CameraIOPhoton} instance using an existing PhotonCamera object, field + * layout, and the robot-to-camera transform. + * + * @param camera The {@link PhotonCamera} instance to use. + * @param layout The {@link AprilTagFieldLayout} describing the field's AprilTag configuration. + * @param robotToCamera The {@link Transform3d} representing the transformation from the robot + * to the camera. + */ public CameraIOPhoton( PhotonCamera camera, AprilTagFieldLayout layout, Transform3d robotToCamera) { this.camera = camera; @@ -34,11 +60,27 @@ public CameraIOPhoton( poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } + /** + * Factory method for creating a {@code CameraIOPhoton} instance for real cameras. + * + * @param params The {@link CameraParams} object containing camera parameters. + * @param layout The {@link AprilTagFieldLayout} describing the field's AprilTag configuration. + * @return A {@code CameraIOPhoton} instance configured for real cameras. + */ public static CameraIOPhoton fromRealCameraParams( CameraParams params, AprilTagFieldLayout layout) { return new CameraIOPhoton(params.name(), layout, params.robotToCamera()); } + /** + * Factory method for creating a {@code CameraIOPhoton} instance for simulated cameras. + * + * @param params The {@link CameraParams} object containing camera parameters. + * @param layout The {@link AprilTagFieldLayout} describing the field's AprilTag configuration. + * @param sim The {@link VisionSystemSim} instance managing the simulation. + * @param stream Whether to enable raw and processed streams for the simulation. + * @return A {@code CameraIOPhoton} instance configured for simulated cameras. + */ public static CameraIOPhoton fromSimCameraParams( CameraParams params, AprilTagFieldLayout layout, VisionSystemSim sim, boolean stream) { PhotonCamera camera = new PhotonCamera(params.name()); @@ -57,6 +99,11 @@ public static CameraIOPhoton fromSimCameraParams( return new CameraIOPhoton(camera, layout, params.robotToCamera()); } + /** + * Updates the {@link CameraIOInputs} object with the latest data from the PhotonVision camera. + * + * @param inputs The {@link CameraIOInputs} object to populate with the latest camera data. + */ @Override public void updateInputs(CameraIOInputs inputs) { inputs.connected = camera.isConnected(); @@ -86,17 +133,13 @@ public void updateInputs(CameraIOInputs inputs) { }); } - // NOTE: Can be used in 2025 code just not ready yet - // private Optional getEstimatedPose () { - // Optional visionEstimate = Optional.empty(); - - // for (var change : camera.getAllUnreadResults()) { - // visionEstimate = poseEstimator.update(change); - // } - - // return visionEstimate; - // } - + /** + * Calculates the average distance from the robot to detected AprilTags. + * + * @param pose The {@link EstimatedRobotPose} containing the detected tags. + * @param inputs The {@link CameraIOInputs} object to populate with the calculated distance and + * tag count. + */ private void calculateAverageTagDistance(EstimatedRobotPose pose, CameraIOInputs inputs) { double distance = 0.0; int numTags = 0; @@ -118,6 +161,12 @@ private void calculateAverageTagDistance(EstimatedRobotPose pose, CameraIOInputs inputs.averageTagDistanceM = distance; } + /** + * Calculates the average yaw of detected AprilTags. + * + * @param pose The {@link EstimatedRobotPose} containing the detected tags. + * @return The average yaw as a {@link Rotation2d}. + */ private static Rotation2d calculateAverageTagYaw(EstimatedRobotPose pose) { double yawRad = 0.0; for (PhotonTrackedTarget target : pose.targetsUsed) { diff --git a/vision/src/main/java/coppercore/vision/CameraParams.java b/vision/src/main/java/coppercore/vision/CameraParams.java index a92b1f5..3131465 100644 --- a/vision/src/main/java/coppercore/vision/CameraParams.java +++ b/vision/src/main/java/coppercore/vision/CameraParams.java @@ -3,6 +3,16 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; +/** + * Represents the configuration parameters for a camera. + * + * @param name The camera name. + * @param xResolution Horizontal resolution in pixels. + * @param yResolution Vertical resolution in pixels. + * @param fps Frames per second. + * @param fov Field of view as a {@link Rotation2d}. + * @param robotToCamera Transform from the robot to the camera as a {@link Transform3d}. + */ public record CameraParams( String name, int xResolution, diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java index 47f8390..8441051 100644 --- a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java +++ b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java @@ -11,7 +11,7 @@ * per-project. */ public final class CoreVisionConstants { - // TODO: Tune this value + // TODO: Tune this value. // This value mainly exists so that this file stays named CoreVisionConstants. public static final double maxAcceptedDistanceMeters = 10.0; public static final Matrix singleTagStdDev = VecBuilder.fill(1, 1, 1); diff --git a/vision/src/main/java/coppercore/vision/LayoutUtil.java b/vision/src/main/java/coppercore/vision/LayoutUtil.java index 368fc7e..fd14e89 100644 --- a/vision/src/main/java/coppercore/vision/LayoutUtil.java +++ b/vision/src/main/java/coppercore/vision/LayoutUtil.java @@ -6,11 +6,18 @@ import java.io.IOException; import java.util.Collections; +/** Utility class for loading an AprilTag field layout from a JSON file. */ public class LayoutUtil { + /** + * Initializes an AprilTag field layout from a JSON file located in the deploy directory. If the + * layout fails to load, an empty layout is returned and a warning is logged. + * + * @param name The name of the layout file (without the .json extension). + * @return The initialized {@link AprilTagFieldLayout}. + */ public static AprilTagFieldLayout initLayout(String name) { AprilTagFieldLayout layout; // AprilTagFieldLayout's constructor throws an IOException, so we must catch it - // in order to initialize our layout as a static constant try { layout = new AprilTagFieldLayout( diff --git a/vision/src/main/java/coppercore/vision/VisionLocalizer.java b/vision/src/main/java/coppercore/vision/VisionLocalizer.java index fb8b21e..d8731bd 100644 --- a/vision/src/main/java/coppercore/vision/VisionLocalizer.java +++ b/vision/src/main/java/coppercore/vision/VisionLocalizer.java @@ -7,17 +7,27 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.Consumer; +/** + * Localizes the robot using camera measurements. Periodically updates camera data and allows for + * custom handling of new measurements. + */ public class VisionLocalizer extends SubsystemBase { private CameraContainer container; - // avoid NullPointerExceptions by setting a default no-op + // Default no-op consumer to avoid NullPointerExceptions private Consumer cameraConsumer = (c) -> {}; + /** + * Constructs a VisionLocalizer with a given camera container. + * + * @param container The container holding cameras for localization. + */ public VisionLocalizer(CameraContainer container) { this.container = container; } + /** Periodically updates the camera data and processes new measurements. */ @Override public void periodic() { container.update(); @@ -28,18 +38,25 @@ public void periodic() { } } + /** + * Sets a custom consumer for handling camera measurements. + * + * @param cameraConsumer The consumer to handle new camera measurements. + */ public void setCameraConsumer(Consumer cameraConsumer) { this.cameraConsumer = cameraConsumer; } + /** + * Checks if the coprocessor is connected. + * + * @return True if the coprocessor is connected, otherwise false. + */ public boolean coprocessorConnected() { return container.getCameras().get(0).isConnected(); } - /** - * This class exists solely because java has no functional interface for a function with 3 - * inputs - */ + /** Represents a camera measurement with a pose, timestamp, and variance. */ public static record CameraMeasurement( Pose3d pose, double timestamp, Matrix variance) {} }