Merge pull request #72 from Manchester-Central/camera-inheritance-fix

Camera inheritance fix

Author: Pathal

src/main/java/com/chaos131/vision/AbstractChaosCamera.java

@@ -66,7 +66,7 @@ public abstract class AbstractChaosCamera extends SubsystemBase {
   /**
    * A list of April Tags we care about. This is typically the list of all april tags on the field.
    */
-  protected ArrayList<Quad> m_localizationTags = new ArrayList<Quad>();
+  protected ArrayList<? extends Quad> m_localizationTags = new ArrayList<Quad>();
 
   /**
    * There are only 3 core modes for cameras in FRC: localization, tracking a game piece, mapping
@@ -177,7 +177,7 @@ public AbstractChaosCamera setRobotRotationSupplier(Supplier<Double> rotationSup
    * @param tags list of quads describing the april tags
    * @return the class itself for chaining
    */
-  public AbstractChaosCamera setTagsOfInterest(ArrayList<Quad> tags) {
+  public AbstractChaosCamera setTagsOfInterest(ArrayList<? extends Quad> tags) {
     m_localizationTags = tags;
     return this;
   }

src/main/java/com/chaos131/vision/CameraTransforms.java

@@ -127,7 +127,7 @@ public static Matrix<N4, N4> FrustumProjectionMatrix(
    * @return a list of 4 element vectors that are in view of the camera
    */
   public static ArrayList<Matrix<N4, N1>> CalculateVisibleCoordinates(
-      Pose3d pose, Quad[] quads, double hfov, double vfov) {
+      Pose3d pose, ArrayList<? extends Quad> quads, double hfov, double vfov) {
     return CalculateVisibleCoordinates(pose, quads, hfov, vfov, 0.05, 100);
   }
 
@@ -143,7 +143,7 @@ public static ArrayList<Matrix<N4, N1>> CalculateVisibleCoordinates(
    * @return a list of 4 element vectors that are in view of the camera
    */
   public static ArrayList<Matrix<N4, N1>> CalculateVisibleCoordinates(
-      Pose3d pose, Quad[] quads, double hfov, double vfov, double n, double f) {
+      Pose3d pose, ArrayList<? extends Quad> quads, double hfov, double vfov, double n, double f) {
     var view_projection_matrix = FrustumProjectionMatrix(hfov, vfov, n, f).times(ViewMatrix(pose));
     ArrayList<Matrix<N4, N1>> final_points = new ArrayList<>();
     for (Quad q : quads) {
@@ -174,7 +174,7 @@ public static ArrayList<Matrix<N4, N1>> CalculateVisibleCoordinates(
    * @return a list of 4 element vectors that are in view of the camera
    */
   public static ArrayList<Matrix<N4, N1>> CalculateVisibleCoordinates(
-      Pose3d pose, Quad[] quads, Matrix<N4, N4> view_proj) {
+      Pose3d pose, ArrayList<? extends Quad> quads, Matrix<N4, N4> view_proj) {
     ArrayList<Matrix<N4, N1>> final_points = new ArrayList<>();
     for (Quad q : quads) {
       var points = q.getPoints();

src/main/java/com/chaos131/vision/LimelightCamera.java

@@ -5,7 +5,6 @@
 import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
-import com.chaos131.util.Quad;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -289,7 +288,7 @@ public void updateCropFromRobotpose(
       Pose3d robotPose, double horizontal_margin, double vertical_margin) {
     var visibletags =
         CameraTransforms.CalculateVisibleCoordinates(
-            robotPose, (Quad[]) m_localizationTags.toArray(), EPSILON, EPSILON);
+            robotPose, m_localizationTags, EPSILON, EPSILON);
     // returns in the order of [minX, minY, maxX, maxY]
     var bounds = CameraTransforms.FindBounds(visibletags, horizontal_margin, vertical_margin);
 

src/test/java/com/chaos131/vision/CameraTransformsTests.java

@@ -18,7 +18,8 @@ public class CameraTransformsTests {
   @Test
   public void testCalculateVisibleCoordinates() {
     var source_tag = new Translation3d(-0.0381, 5.547868, 1.368552);
-    var tag_coords = new Quad[] {new Quad(source_tag, source_tag, source_tag, source_tag)};
+    var tag_coords = new ArrayList<Quad>();
+    tag_coords.add(new Quad(source_tag, source_tag, source_tag, source_tag));
 
     var camerapose =
         new Pose3d(new Translation3d(4.0, 5.547868, 1.368552), new Rotation3d(0, 0, Math.PI));
@@ -102,9 +103,7 @@ public void testAllTagsInView() {
             new Translation3d(22.0, field_up_meters / 2, 1.451102), // z is the speaker tag height
             new Rotation3d(0, 0, Math.PI));
     ArrayList<AprilTag> fmap_data = FieldData.LoadTagLocations(fmap.fmap2024);
-    AprilTag[] tags = new AprilTag[fmap_data.size()];
-    fmap_data.toArray(tags);
-    var res = CameraTransforms.CalculateVisibleCoordinates(camerapose, tags, 80, 56, 0.2, 100);
+    var res = CameraTransforms.CalculateVisibleCoordinates(camerapose, fmap_data, 80, 56, 0.2, 100);
     // String desmos = "";
     // for (var p : res) {
     //   desmos += "(" + p.get(0, 0) + "," + p.get(1, 0) + "),";
@@ -116,7 +115,8 @@ public void testAllTagsInView() {
   @Test
   public void testCropSpace() {
     var speaker_tag = new Translation3d(-0.0381, 5.547868, 1.368552);
-    var tag_coords = new Quad[] {new Quad(speaker_tag, speaker_tag, speaker_tag, speaker_tag)};
+    var tag_coords = new ArrayList<Quad>();
+    tag_coords.add(new Quad(speaker_tag, speaker_tag, speaker_tag, speaker_tag));
 
     var camerapose =
         new Pose3d(new Translation3d(5.0, 5.547868, 1.368552), new Rotation3d(0, 0, Math.PI));