Clean Object Detection

src/main/java/frc/trigon/robot/Robot.java

@@ -11,6 +11,7 @@
 import frc.trigon.lib.hardware.RobotHardwareStats;
 import frc.trigon.lib.hardware.phoenix6.Phoenix6Inputs;
 import frc.trigon.robot.constants.RobotConstants;
+import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -59,6 +60,7 @@ public void testInit() {
 
     @Override
     public void simulationPeriodic() {
+        SimulationFieldHandler.update();
     }
 
     @Override

src/main/java/frc/trigon/robot/RobotContainer.java

@@ -16,18 +16,17 @@
 import frc.trigon.robot.constants.CameraConstants;
 import frc.trigon.robot.constants.LEDConstants;
 import frc.trigon.robot.constants.OperatorConstants;
-import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
+import frc.trigon.robot.misc.objectDetection.ObjectPoseEstimator;
 import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
-import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
+import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimator;
 import frc.trigon.robot.subsystems.MotorSubsystem;
 import frc.trigon.robot.subsystems.swerve.Swerve;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 public class RobotContainer {
-    public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator();
+    public static final RobotPoseEstimator ROBOT_POSE_ESTIMATOR = new RobotPoseEstimator();
     public static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = new ObjectPoseEstimator(
             CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS,
-            ObjectPoseEstimator.DistanceCalculationMethod.ROTATION_AND_TRANSLATION,
             SimulatedGamePieceConstants.GamePieceType.GAME_PIECE_TYPE,
             CameraConstants.OBJECT_DETECTION_CAMERA
     );

src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java

@@ -7,8 +7,8 @@
 import frc.trigon.lib.utilities.flippable.FlippableRotation2d;
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
+import frc.trigon.robot.misc.objectDetection.ObjectPoseEstimator;
 import frc.trigon.robot.constants.AutonomousConstants;
-import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
 import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
 import org.littletonrobotics.junction.Logger;

src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java

@@ -106,7 +106,7 @@ private boolean hasNoTrackedGamePiece() {
     }
 
     private void trackGamePiece() {
-        distanceFromTrackedGamePiece = RobotContainer.OBJECT_POSE_ESTIMATOR.hasObject() ?
+        distanceFromTrackedGamePiece = RobotContainer.OBJECT_POSE_ESTIMATOR.hasObjects() ?
                 calculateSelfRelativeDistanceFromBestGamePiece() :
                 null;
     }

src/main/java/frc/trigon/robot/constants/CameraConstants.java

@@ -4,7 +4,7 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
-import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera;
+import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCamera;
 
 public class CameraConstants {
     private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d(//TODO: AHAHAHAHAH

src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java → src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java

@@ -1,8 +1,8 @@
-package frc.trigon.robot.misc.objectdetectioncamera;
+package frc.trigon.robot.misc.objectDetection;
 
 import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
 
-public class ObjectDetectionCameraConstants {
+public class ObjectDetectionConstants {
     public static final int NUMBER_OF_GAME_PIECE_TYPES = SimulatedGamePieceConstants.GamePieceType.values().length;
     static final double TRACKED_OBJECT_TOLERANCE_METERS = 0.12;
-}
+}

src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java

@@ -0,0 +1,220 @@
+package frc.trigon.robot.misc.objectDetection;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCamera;
+import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
+import org.littletonrobotics.junction.Logger;
+
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.HashSet;
+import java.util.Set;
+
+public class ObjectPoseEstimator extends SubsystemBase {
+    private final double deletionThresholdSeconds;
+    private final SimulatedGamePieceConstants.GamePieceType gamePieceType;
+    private final ObjectDetectionCamera camera;
+    /**
+     * Stores the position of each detected object along with the timestamp of when it was detected.
+     */
+    private final HashMap<Translation2d, Double> objectPositionsToDetectionTimestamp;
+
+    /**
+     * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera.
+     *
+     * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed
+     * @param gamePieceType            the type of game piece to track
+     * @param camera                   the camera used for detecting objects
+     */
+    public ObjectPoseEstimator(double deletionThresholdSeconds,
+                               SimulatedGamePieceConstants.GamePieceType gamePieceType,
+                               ObjectDetectionCamera camera) {
+        this.deletionThresholdSeconds = deletionThresholdSeconds;
+        this.gamePieceType = gamePieceType;
+        this.camera = camera;
+        this.objectPositionsToDetectionTimestamp = new HashMap<>();
+    }
+
+    /**
+     * Updates the object positions based on the camera detected objects.
+     * Removes objects that have not been detected for a certain time frame, defined in {@link ObjectPoseEstimator#deletionThresholdSeconds}.
+     */
+    @Override
+    public void periodic() {
+        updateTrackedObjectsPositions();
+        removeOldObjects();
+        Logger.recordOutput("ObjectPoseEstimator/knownObjectPositions", getObjectsOnField().toArray(Translation2d[]::new));
+    }
+
+    /**
+     * Gets the position of all known objects on the field.
+     *
+     * @return a list of Translation2d representing the positions of objects on the field
+     */
+    public ArrayList<Translation2d> getObjectsOnField() {
+        return new ArrayList<>(objectPositionsToDetectionTimestamp.keySet());
+    }
+
+    /**
+     * Removes the closest object to the robot from the list of objects in the pose estimator.
+     */
+    public void removeClosestObjectToRobot() {
+        final Translation2d closestObject = getClosestObjectToRobot();
+        if (closestObject == null)
+            return;
+        removeObject(closestObject);
+    }
+
+    /**
+     * Removes the closest object to the intake from the list of objects in the pose estimator.
+     *
+     * @param intakeTransform the transform of the intake relative to the robot
+     */
+    public void removeClosestObjectToIntake(Transform2d intakeTransform) {
+        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        final Translation2d closestObjectToIntake = getClosestTrackedObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation());
+        if (closestObjectToIntake == null)
+            return;
+        removeObject(closestObjectToIntake);
+    }
+
+    /**
+     * Removes the closest object to a given pose from the list of objects in the pose estimator.
+     *
+     * @param fieldRelativePose the pose to which the removed object is closest
+     */
+    public void removeClosestObjectToPose(Pose2d fieldRelativePose) {
+        removeClosestObjectToPosition(fieldRelativePose.getTranslation());
+    }
+
+    /**
+     * Removes the closest object to a given position from the stored objects in the pose estimator.
+     *
+     * @param position the position to which the removed object is closest
+     */
+    public void removeClosestObjectToPosition(Translation2d position) {
+        final Translation2d closestObject = getClosestTrackedObjectToPosition(position);
+        if (closestObject == null)
+            return;
+        removeObject(closestObject);
+    }
+
+    /**
+     * Removes a specific object from the stored objects in the pose estimator.
+     * Unlike {@link #removeClosestObjectToPosition} which removes the closest object to a given position.
+     *
+     * @param objectPosition the position of the object to be removed. Must be the precise position as stored in the pose estimator.
+     */
+    public void removeObject(Translation2d objectPosition) {
+        objectPositionsToDetectionTimestamp.remove(objectPosition);
+    }
+
+    /**
+     * Returns whether any objects are stored in the pose estimator.
+     *
+     * @return if there are objects stored in the pose estimator
+     */
+    public boolean hasObjects() {
+        return !objectPositionsToDetectionTimestamp.isEmpty();
+    }
+
+    /**
+     * Gets the position of the closest object to the robot.
+     *
+     * @return the closest object to the robot
+     */
+    public Translation2d getClosestObjectToRobot() {
+        return getClosestTrackedObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation());
+    }
+
+    private void updateTrackedObjectsPositions() {
+        final Translation2d[] visibleObjects = camera.getObjectPositionsOnField(gamePieceType);
+        final HashMap<Translation2d, Translation2d> trackedObjectsToUpdatedPositions = new HashMap<>();
+
+        for (Translation2d visibleObject : visibleObjects)
+            updateObjectPosition(visibleObject, trackedObjectsToUpdatedPositions);
+
+        applyObjectUpdates(trackedObjectsToUpdatedPositions);
+    }
+
+    private void applyObjectUpdates(HashMap<Translation2d, Translation2d> currentToNewObjectPositions) {
+        final double currentTimestamp = Timer.getTimestamp();
+
+        objectPositionsToDetectionTimestamp.keySet().removeAll(currentToNewObjectPositions.keySet());
+        currentToNewObjectPositions.values().forEach(object -> objectPositionsToDetectionTimestamp.put(object, currentTimestamp));
+    }
+
+    private void updateObjectPosition(Translation2d objectUpdate, HashMap<Translation2d, Translation2d> trackedObjectsToUpdatedPositions) {
+        final Translation2d closestAvailableTrackedObjectToVisibleObject = getClosestAvailableObjectToUpdate(objectUpdate, trackedObjectsToUpdatedPositions);
+        if (closestAvailableTrackedObjectToVisibleObject == null) {
+            trackedObjectsToUpdatedPositions.put(objectUpdate, objectUpdate);
+            return;
+        }
+        final Translation2d previousUpdate = trackedObjectsToUpdatedPositions.get(closestAvailableTrackedObjectToVisibleObject);
+        trackedObjectsToUpdatedPositions.put(closestAvailableTrackedObjectToVisibleObject, objectUpdate);
+        if (previousUpdate != null)
+            updateObjectPosition(previousUpdate, trackedObjectsToUpdatedPositions);
+    }
+
+    private Translation2d getClosestAvailableObjectToUpdate(Translation2d update, HashMap<Translation2d, Translation2d> objectsWithUpdates) {
+        final Set<Translation2d> availableObjectsToUpdate = getAvailableObjectsToUpdate(update, objectsWithUpdates);
+        if (availableObjectsToUpdate == null || availableObjectsToUpdate.isEmpty())
+            return null;
+        return getClosestObjectFromSetToPosition(update, availableObjectsToUpdate);
+    }
+
+    private Set<Translation2d> getAvailableObjectsToUpdate(Translation2d update, HashMap<Translation2d, Translation2d> objectsWithUpdates) {
+        if (objectPositionsToDetectionTimestamp.isEmpty())
+            return null;
+        final Set<Translation2d> availableObjects = new HashSet<>();
+        for (Translation2d currentObject : objectPositionsToDetectionTimestamp.keySet()) {
+            final double updateDistanceFromCurrentObject = update.getDistance(currentObject);
+            if (updateDistanceFromCurrentObject > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS)
+                continue;
+            if (!objectsWithUpdates.containsKey(currentObject)) {
+                availableObjects.add(currentObject);
+                continue;
+            }
+            if (isNewUpdateCloserThanPreviousUpdate(update, objectsWithUpdates.get(currentObject), currentObject))
+                availableObjects.add(currentObject);
+        }
+        return availableObjects;
+    }
+
+    private boolean isNewUpdateCloserThanPreviousUpdate(Translation2d newUpdate, Translation2d previousUpdate, Translation2d object) {
+        return newUpdate.getDistance(object) < previousUpdate.getDistance(object);
+    }
+
+    private Translation2d getClosestTrackedObjectToPosition(Translation2d position) {
+        return getClosestObjectFromSetToPosition(position, objectPositionsToDetectionTimestamp.keySet());
+    }
+
+    private Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set<Translation2d> objects) {
+        if (objects.isEmpty())
+            return null;
+        Translation2d closestObjectTranslation = null;
+        double closestObjectDistance = Double.MAX_VALUE;
+
+        for (Translation2d object : objects) {
+            final double currentObjectDistance = position.getDistance(object);
+            if (currentObjectDistance < closestObjectDistance) {
+                closestObjectDistance = currentObjectDistance;
+                closestObjectTranslation = object;
+            }
+        }
+        return closestObjectTranslation;
+    }
+
+    private void removeOldObjects() {
+        objectPositionsToDetectionTimestamp.entrySet().removeIf(entry -> hasObjectExpired(entry.getValue()));
+    }
+
+    private boolean hasObjectExpired(double timestamp) {
+        return Timer.getTimestamp() - timestamp > deletionThresholdSeconds;
+    }
+}

src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java → src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java

@@ -1,13 +1,13 @@
-package frc.trigon.robot.misc.objectdetectioncamera;
+package frc.trigon.robot.misc.objectDetection.objectdetectioncamera;
 
 import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.trigon.lib.hardware.RobotHardwareStats;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.misc.objectdetectioncamera.io.PhotonObjectDetectionCameraIO;
-import frc.trigon.robot.misc.objectdetectioncamera.io.SimulationObjectDetectionCameraIO;
+import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.io.PhotonObjectDetectionCameraIO;
+import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.io.SimulationObjectDetectionCameraIO;
 import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
 import org.littletonrobotics.junction.Logger;
-import frc.trigon.lib.hardware.RobotHardwareStats;
 
 /**
  * An object detection camera is a class that represents a camera that detects objects other than apriltags, most likely game pieces.
@@ -31,51 +31,52 @@ public void periodic() {
     }
 
     /**
-     * Calculates the position of the best object on the field from the 3D rotation of the object relative to the camera.
+     * Calculates the position of the closest object on the field from its 3D rotation relative to the camera.
      * This assumes the object is on the ground.
      * Once it is known that the object is on the ground,
      * one can simply find the transform from the camera to the ground and apply it to the object's rotation.
      *
-     * @return the best object's 2D position on the field (z is assumed to be 0)
+     * @return the closest object's 2D position on the field (z is assumed to be 0)
      */
-    public Translation2d calculateBestObjectPositionOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) {
+    public Translation2d calculateClosestObjectPositionOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) {
         final Translation2d[] targetObjectsTranslation = getObjectPositionsOnField(targetGamePiece);
         final Translation2d currentRobotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation();
         if (targetObjectsTranslation.length == 0)
             return null;
-        Translation2d bestObjectTranslation = targetObjectsTranslation[0];
+        Translation2d closestObjectTranslation = targetObjectsTranslation[0];
+        double closestObjectDistanceToRobot = currentRobotTranslation.getDistance(closestObjectTranslation);
 
-        for (int i = 1; i < targetObjectsTranslation.length; i++) {
-            final Translation2d currentObjectTranslation = targetObjectsTranslation[i];
-            final double bestObjectDifference = currentRobotTranslation.getDistance(bestObjectTranslation);
-            final double currentObjectDifference = currentRobotTranslation.getDistance(currentObjectTranslation);
-            if (currentObjectDifference < bestObjectDifference)
-                bestObjectTranslation = currentObjectTranslation;
+        for (Translation2d currentObjectTranslation : targetObjectsTranslation) {
+            final double currentObjectDistanceToRobot = currentRobotTranslation.getDistance(currentObjectTranslation);
+            if (currentObjectDistanceToRobot < closestObjectDistanceToRobot) {
+                closestObjectTranslation = currentObjectTranslation;
+                closestObjectDistanceToRobot = currentObjectDistanceToRobot;
+            }
         }
-        return bestObjectTranslation;
+        return closestObjectTranslation;
     }
 
-    public boolean hasTargets(SimulatedGamePieceConstants.GamePieceType targetGamePiece) {
-        return objectDetectionCameraInputs.hasTarget[targetGamePiece.id];
+    public boolean hasObject(SimulatedGamePieceConstants.GamePieceType targetGamePiece) {
+        return objectDetectionCameraInputs.hasObject[targetGamePiece.id];
     }
 
     public Translation2d[] getObjectPositionsOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) {
-        final Rotation3d[] visibleObjectsRotations = getTargetObjectsRotations(targetGamePiece);
-        final Translation2d[] objectsPositionsOnField = new Translation2d[visibleObjectsRotations.length];
+        final Rotation3d[] visibleObjectRotations = getObjectsRotations(targetGamePiece);
+        final Translation2d[] objectPositionsOnField = new Translation2d[visibleObjectRotations.length];
 
-        for (int i = 0; i < visibleObjectsRotations.length; i++)
-            objectsPositionsOnField[i] = calculateObjectPositionFromRotation(visibleObjectsRotations[i]);
+        for (int i = 0; i < visibleObjectRotations.length; i++)
+            objectPositionsOnField[i] = calculateObjectPositionFromRotation(visibleObjectRotations[i]);
 
-        Logger.recordOutput("ObjectDetectionCamera/Visible" + targetGamePiece.name(), objectsPositionsOnField);
-        return objectsPositionsOnField;
+        Logger.recordOutput("ObjectDetectionCamera/Visible" + targetGamePiece.name(), objectPositionsOnField);
+        return objectPositionsOnField;
     }
 
-    public Rotation3d[] getTargetObjectsRotations(SimulatedGamePieceConstants.GamePieceType targetGamePiece) {
+    public Rotation3d[] getObjectsRotations(SimulatedGamePieceConstants.GamePieceType targetGamePiece) {
         return objectDetectionCameraInputs.visibleObjectRotations[targetGamePiece.id];
     }
 
     /**
-     * Calculates the position of the object on the field from the 3D rotation of the object relative to the camera.
+     * Calculates the position of an object on the field from its 3D rotation relative to the camera.
      * This assumes the object is on the ground.
      * Once it is known that the object is on the ground,
      * one can simply find the transform from the camera to the ground and apply it to the object's rotation.