From 040edb992238b71fb52746b7ddfcb17d7ea36f5d Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 14:44:32 +0200 Subject: [PATCH 01/68] changed order of class variables to match the order of the constructor --- .../objectdetectioncamera/ObjectPoseEstimator.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index 64ea2cfe..3c14295f 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -13,18 +13,19 @@ import java.util.HashMap; public class ObjectPoseEstimator extends SubsystemBase { - private final HashMap knownObjectPositions; - private final ObjectDetectionCamera camera; private final double deletionThresholdSeconds; private final SimulatedGamePieceConstants.GamePieceType gamePieceType; + private final ObjectDetectionCamera camera; + private final HashMap knownObjectPositions; private final double rotationToTranslation; /** * 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 + * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed + * @param distanceCalculationMethod the method used to calculate the distance from the game piece + * @param gamePieceType the type of game piece to track + * @param camera the camera used for detecting objects */ public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, SimulatedGamePieceConstants.GamePieceType gamePieceType, From dc1b5c5ffa7a3c916f1961a962b4c7cefa88b8a1 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 17:33:00 +0200 Subject: [PATCH 02/68] changed getClosestObjectToPosition to be just. return statement. (thank you claude for the suggestion) --- .../ObjectDetectionCameraConstants.java | 2 +- .../ObjectDetectionCameraIO.java | 2 +- .../ObjectPoseEstimator.java | 19 ++++--------------- .../io/PhotonObjectDetectionCameraIO.java | 2 +- 4 files changed, 7 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java index 046781ad..dd0898b3 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java @@ -7,4 +7,4 @@ public class ObjectDetectionCameraConstants { public static final int NUMBER_OF_GAME_PIECE_TYPES = SimulatedGamePieceConstants.GamePieceType.values().length; static final double TRACKED_OBJECT_TOLERANCE_METERS = 0.12; public static final Rotation2d LOLLIPOP_TOLERANCE = Rotation2d.fromDegrees(3.5); -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java index 4d0f336b..02bdf09e 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java @@ -24,4 +24,4 @@ public static class ObjectDetectionCameraInputs { public Rotation3d[][] visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; public double latestResultTimestamp = 0; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index 3c14295f..25043117 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -10,6 +10,7 @@ import org.littletonrobotics.junction.Logger; import java.util.ArrayList; +import java.util.Comparator; import java.util.HashMap; public class ObjectPoseEstimator extends SubsystemBase { @@ -119,21 +120,9 @@ public Translation2d getClosestObjectToRobot() { * @return the closest object's position on the field, or null if no objects are known */ public Translation2d getClosestObjectToPosition(Translation2d position) { - final Translation2d[] objectsTranslations = knownObjectPositions.keySet().toArray(Translation2d[]::new); - if (knownObjectPositions.isEmpty()) - return null; - Translation2d bestObjectTranslation = objectsTranslations[0]; - double closestObjectDistance = position.getDistance(bestObjectTranslation); - - for (int i = 1; i < objectsTranslations.length; i++) { - final Translation2d currentObjectTranslation = objectsTranslations[i]; - final double currentObjectDistance = position.getDistance(currentObjectTranslation); - if (currentObjectDistance < closestObjectDistance) { - closestObjectDistance = currentObjectDistance; - bestObjectTranslation = currentObjectTranslation; - } - } - return bestObjectTranslation; + return knownObjectPositions.keySet().stream() + .min(Comparator.comparingDouble(position::getDistance)) + .orElse(null); } /** diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java index 541f10e8..73a5cf7b 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -82,4 +82,4 @@ private Rotation3d extractRotation3d(PhotonTrackedTarget target) { Units.degreesToRadians(-target.getYaw()) ); } -} +} \ No newline at end of file From b7940fe1b3fd62a3f8d4b0adf8c4e3b76f4bcfde Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 18:08:52 +0200 Subject: [PATCH 03/68] cleaned calculateObjectDistanceRating instead of comparing the x and y of two translations i compared the translations directly using wpilibs .minus function --- .../ObjectPoseEstimator.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index 25043117..ee4902a9 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -126,20 +126,20 @@ public Translation2d getClosestObjectToPosition(Translation2d position) { } /** - * Calculates the "distance rating" of an object. + * Calculates the "distance rating" of an object from a given pose. * The "distance rating" is a unit used to calculate the distance between 2 poses. * It factors in both translation and rotation differences by scaling the units depending on the {@link DistanceCalculationMethod}. * * @param objectTranslation the translation of the object on the field * @param pose the pose to which the distance is measured from - * @return the objects "distance rating" + * @return the objects "distance rating" from the given pose */ - private double calculateObjectDistanceRating(Translation2d objectTranslation, Pose2d pose) { - final double translationDifference = pose.getTranslation().getDistance(objectTranslation); - final double xDifference = Math.abs(pose.getX() - objectTranslation.getX()); - final double yDifference = Math.abs(pose.getY() - objectTranslation.getY()); - final double rotationDifferenceDegrees = Math.abs(pose.getRotation().getDegrees() - Math.atan2(yDifference, xDifference)); - return translationDifference * rotationToTranslation + rotationDifferenceDegrees * (1 - rotationToTranslation); + private double calculateObjectDistanceRatingFromPose(Translation2d objectTranslation, Pose2d pose) { + final Translation2d poseTranslation = pose.getTranslation(); + final double translationDistance = poseTranslation.getDistance(objectTranslation); + final Translation2d translationDifference = objectTranslation.minus(poseTranslation); + final double rotationDifferenceDegrees = Math.abs(pose.getRotation().minus(translationDifference.getAngle()).getDegrees()); + return translationDistance * rotationToTranslation + rotationDifferenceDegrees * (1 - rotationToTranslation); } private void updateObjectPositions() { From dbb4869db4190003ba57c49e2c6cb25b88dee346 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 18:41:54 +0200 Subject: [PATCH 04/68] cleaned updateObjectPositions switched out for loop with stream --- .../ObjectPoseEstimator.java | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index ee4902a9..3c8e3575 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -144,20 +144,18 @@ private double calculateObjectDistanceRatingFromPose(Translation2d objectTransla private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); + for (Translation2d visibleObject : camera.getObjectPositionsOnField(gamePieceType)) { - Translation2d closestObjectToVisibleObject = new Translation2d(); - double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; - - for (Translation2d knownObject : knownObjectPositions.keySet()) { - final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject); - if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { - closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; - closestObjectToVisibleObject = knownObject; - } + final Translation2d closestObjectToVisibleObject = knownObjectPositions.keySet().stream() + .min(Comparator.comparingDouble(visibleObject::getDistance)).orElse(null); + if (closestObjectToVisibleObject != null) { + final double closestObjectToVisibleObjectDistanceMeters = closestObjectToVisibleObject.getDistance(visibleObject); + final boolean isObjectWithinTolerance = closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS; + final boolean isNotAlreadyUpdated = knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp; + + if (isObjectWithinTolerance && isNotAlreadyUpdated) + removeObject(closestObjectToVisibleObject); } - - if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) - removeObject(closestObjectToVisibleObject); knownObjectPositions.put(visibleObject, currentTimestamp); } } From 3467107745f7cb1c5320c5f9a9ac3bd9ac641a44 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 18:43:02 +0200 Subject: [PATCH 05/68] this shouldnt be in the template --- .../objectdetectioncamera/ObjectDetectionCameraConstants.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java index dd0898b3..35792f8b 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java @@ -1,10 +1,8 @@ package frc.trigon.robot.misc.objectdetectioncamera; -import edu.wpi.first.math.geometry.Rotation2d; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; public class ObjectDetectionCameraConstants { public static final int NUMBER_OF_GAME_PIECE_TYPES = SimulatedGamePieceConstants.GamePieceType.values().length; static final double TRACKED_OBJECT_TOLERANCE_METERS = 0.12; - public static final Rotation2d LOLLIPOP_TOLERANCE = Rotation2d.fromDegrees(3.5); } \ No newline at end of file From b7a14a2531c51eae321bb9a4763d5741e77a8338 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 19:55:23 +0200 Subject: [PATCH 06/68] renamed stuff --- .../ObjectDetectionCamera.java | 24 +++++++++---------- .../ObjectDetectionCameraIO.java | 4 ++-- .../io/PhotonObjectDetectionCameraIO.java | 6 ++--- .../io/SimulationObjectDetectionCameraIO.java | 6 ++--- 4 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java index ac9fbe27..120d817d 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java @@ -31,32 +31,32 @@ 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]; 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; + final double closestObjectDistanceToRobot = currentRobotTranslation.getDistance(closestObjectTranslation); + final double currentObjectDistanceToRobot = currentRobotTranslation.getDistance(currentObjectTranslation); + if (currentObjectDistanceToRobot < closestObjectDistanceToRobot) + closestObjectTranslation = currentObjectTranslation; } - return bestObjectTranslation; + return closestObjectTranslation; } - public boolean hasTargets(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { - return objectDetectionCameraInputs.hasTarget[targetGamePiece.id]; + public boolean seesObject(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + return objectDetectionCameraInputs.seesObject[targetGamePiece.id]; } public Translation2d[] getObjectPositionsOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { @@ -75,7 +75,7 @@ public Rotation3d[] getTargetObjectsRotations(SimulatedGamePieceConstants.GamePi } /** - * Calculates the position of the object on the field from the 3D rotation of the object relative to the camera. + * Calculates the position of the 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. diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java index 02bdf09e..41dbb6e0 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java @@ -13,9 +13,9 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { @AutoLog public static class ObjectDetectionCameraInputs { /** - * Whether there is at least one target or not for each game piece, by game piece index (type). + * Whether there is at least one object or not for each game piece, by game piece index (type). */ - public boolean[] hasTarget = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + public boolean[] seesObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; /** * Stores the Rotation3d of all visible objects. * The first index is the game piece ID (type). diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java index 73a5cf7b..6fd5cdc7 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -43,7 +43,7 @@ private PhotonPipelineResult getLatestPipelineResult() { } private void updateNoNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - inputs.hasTarget = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + inputs.seesObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; } @@ -51,14 +51,14 @@ private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inpu final List[] visibleObjectsRotations = new List[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) visibleObjectsRotations[i] = new ArrayList<>(); - Arrays.fill(inputs.hasTarget, false); + Arrays.fill(inputs.seesObject, false); inputs.latestResultTimestamp = result.getTimestampSeconds(); for (PhotonTrackedTarget currentTarget : result.getTargets()) { if (currentTarget.getDetectedObjectClassID() == -1) continue; - inputs.hasTarget[currentTarget.getDetectedObjectClassID()] = true; + inputs.seesObject[currentTarget.getDetectedObjectClassID()] = true; visibleObjectsRotations[currentTarget.getDetectedObjectClassID()].add(extractRotation3d(currentTarget)); } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index a7bd8346..cb282089 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -35,8 +35,8 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { boolean hasAnyTarget = false; for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) { - inputs.hasTarget[i] = !visibleGamePieces[i].isEmpty(); - if (inputs.hasTarget[i]) + inputs.seesObject[i] = !visibleGamePieces[i].isEmpty(); + if (inputs.seesObject[i]) hasAnyTarget = true; } @@ -56,7 +56,7 @@ private ArrayList>[] calculateAllVisibleGam } private void updateNoNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - inputs.hasTarget = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + inputs.seesObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; } From ca9bc12fb508c5c3e3e6ea689d74b12b763d9829 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 19:56:00 +0200 Subject: [PATCH 07/68] Revert "cleaned updateObjectPositions" This reverts commit dbb4869db4190003ba57c49e2c6cb25b88dee346. --- .../ObjectPoseEstimator.java | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index 3c8e3575..ee4902a9 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -144,18 +144,20 @@ private double calculateObjectDistanceRatingFromPose(Translation2d objectTransla private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); - for (Translation2d visibleObject : camera.getObjectPositionsOnField(gamePieceType)) { - final Translation2d closestObjectToVisibleObject = knownObjectPositions.keySet().stream() - .min(Comparator.comparingDouble(visibleObject::getDistance)).orElse(null); - if (closestObjectToVisibleObject != null) { - final double closestObjectToVisibleObjectDistanceMeters = closestObjectToVisibleObject.getDistance(visibleObject); - final boolean isObjectWithinTolerance = closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS; - final boolean isNotAlreadyUpdated = knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp; - - if (isObjectWithinTolerance && isNotAlreadyUpdated) - removeObject(closestObjectToVisibleObject); + Translation2d closestObjectToVisibleObject = new Translation2d(); + double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; + + for (Translation2d knownObject : knownObjectPositions.keySet()) { + final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject); + if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { + closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; + closestObjectToVisibleObject = knownObject; + } } + + if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) + removeObject(closestObjectToVisibleObject); knownObjectPositions.put(visibleObject, currentTimestamp); } } From 9e66d2363704bece7ab7d680b6ce7a37d451bf87 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 20:06:48 +0200 Subject: [PATCH 08/68] got rid of stream --- .../ObjectPoseEstimator.java | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index ee4902a9..c6135152 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -120,9 +120,21 @@ public Translation2d getClosestObjectToRobot() { * @return the closest object's position on the field, or null if no objects are known */ public Translation2d getClosestObjectToPosition(Translation2d position) { - return knownObjectPositions.keySet().stream() - .min(Comparator.comparingDouble(position::getDistance)) - .orElse(null); + final Translation2d[] objectsTranslations = knownObjectPositions.keySet().toArray(Translation2d[]::new); + if (knownObjectPositions.isEmpty()) + return null; + Translation2d closestObjectTranslation = objectsTranslations[0]; + double closestObjectDistance = position.getDistance(closestObjectTranslation); + + for (int i = 1; i < objectsTranslations.length; i++) { + final Translation2d currentObjectTranslation = objectsTranslations[i]; + final double currentObjectDistance = position.getDistance(currentObjectTranslation); + if (currentObjectDistance < closestObjectDistance) { + closestObjectDistance = currentObjectDistance; + closestObjectTranslation = currentObjectTranslation; + } + } + return closestObjectTranslation; } /** From 1173f306a10a0cde96f580a431d4281bf9eedd88 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 20:09:44 +0200 Subject: [PATCH 09/68] Update ObjectDetectionCamera.java --- .../misc/objectdetectioncamera/ObjectDetectionCamera.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java index 120d817d..8e346a45 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java @@ -60,7 +60,7 @@ public boolean seesObject(SimulatedGamePieceConstants.GamePieceType targetGamePi } public Translation2d[] getObjectPositionsOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { - final Rotation3d[] visibleObjectsRotations = getTargetObjectsRotations(targetGamePiece); + final Rotation3d[] visibleObjectsRotations = getObjectsRotations(targetGamePiece); final Translation2d[] objectsPositionsOnField = new Translation2d[visibleObjectsRotations.length]; for (int i = 0; i < visibleObjectsRotations.length; i++) @@ -70,7 +70,7 @@ public Translation2d[] getObjectPositionsOnField(SimulatedGamePieceConstants.Gam return objectsPositionsOnField; } - public Rotation3d[] getTargetObjectsRotations(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + public Rotation3d[] getObjectsRotations(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { return objectDetectionCameraInputs.visibleObjectRotations[targetGamePiece.id]; } From 3fe3d92c8f2e7f698873f4f4c0b016c4278eac07 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 20:18:01 +0200 Subject: [PATCH 10/68] Update SimulationObjectDetectionCameraIO.java --- .../io/SimulationObjectDetectionCameraIO.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index cb282089..bd537cbe 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -51,7 +51,7 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { private ArrayList>[] calculateAllVisibleGamePieces(Pose3d cameraPose) { final ArrayList>[] visibleGamePieces = new ArrayList[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; for (int i = 0; i < visibleGamePieces.length; i++) - visibleGamePieces[i] = calculateVisibleGamePieces(cameraPose, i); + visibleGamePieces[i] = calculateVisibleGamePiecesPositions(cameraPose, i); return visibleGamePieces; } @@ -79,7 +79,7 @@ private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inpu * @param objectID the ID of the object to check for visibility * @return the placements of the visible objects, as a pair of the object and the rotation of the object relative to the camera */ - private ArrayList> calculateVisibleGamePieces(Pose3d cameraPose, int objectID) { + private ArrayList> calculateVisibleGamePiecesPositions(Pose3d cameraPose, int objectID) { final ArrayList gamePiecesOnField = SimulationFieldHandler.getSimulatedGamePieces(); final ArrayList> visibleTargetObjects = new ArrayList<>(); for (SimulatedGamePiece currentObject : gamePiecesOnField) { @@ -87,7 +87,7 @@ private ArrayList> calculateVisibleGamePiec continue; final Rotation3d cameraAngleToObject = calculateCameraAngleToObject(currentObject.getPose(), cameraPose); - if (isWithinFOV(cameraAngleToObject)) + if (isObjectWithinFOV(cameraAngleToObject)) visibleTargetObjects.add(new Pair<>(currentObject, cameraAngleToObject)); } @@ -137,7 +137,7 @@ private Rotation2d getPitch(Translation3d vector) { * @param objectRotation the rotation of the object relative to the camera * @return if the object is within the field-of-view of the camera */ - private boolean isWithinFOV(Rotation3d objectRotation) { + private boolean isObjectWithinFOV(Rotation3d objectRotation) { return Math.abs(objectRotation.getZ()) <= CAMERA_HORIZONTAL_FOV.getRadians() / 2 && Math.abs(objectRotation.getY()) <= CAMERA_VERTICAL_FOV.getRadians() / 2; } From f8e5c7a4096fe744e6936ffb82678a80c6132d78 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 23:02:41 +0200 Subject: [PATCH 11/68] added code to use more than one camera --- .../ObjectDetectionCamera.java | 4 +- .../ObjectDetectionCameraIO.java | 2 +- .../ObjectPoseEstimator.java | 46 +++++++++++-------- .../io/PhotonObjectDetectionCameraIO.java | 6 +-- .../io/SimulationObjectDetectionCameraIO.java | 10 ++-- 5 files changed, 39 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java index 8e346a45..ed9c3704 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java @@ -55,8 +55,8 @@ public Translation2d calculateClosestObjectPositionOnField(SimulatedGamePieceCon return closestObjectTranslation; } - public boolean seesObject(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { - return objectDetectionCameraInputs.seesObject[targetGamePiece.id]; + public boolean hasObject(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + return objectDetectionCameraInputs.hasObject[targetGamePiece.id]; } public Translation2d[] getObjectPositionsOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java index 41dbb6e0..19ad356f 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java @@ -15,7 +15,7 @@ public static class ObjectDetectionCameraInputs { /** * Whether there is at least one object or not for each game piece, by game piece index (type). */ - public boolean[] seesObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + public boolean[] hasObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; /** * Stores the Rotation3d of all visible objects. * The first index is the game piece ID (type). diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index c6135152..0a1ca51c 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -10,13 +10,14 @@ import org.littletonrobotics.junction.Logger; import java.util.ArrayList; -import java.util.Comparator; 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; + private final ObjectDetectionCamera[] cameras; private final HashMap knownObjectPositions; private final double rotationToTranslation; @@ -26,14 +27,14 @@ public class ObjectPoseEstimator extends SubsystemBase { * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed * @param distanceCalculationMethod the method used to calculate the distance from the game piece * @param gamePieceType the type of game piece to track - * @param camera the camera used for detecting objects + * @param cameras the camera used for detecting objects */ public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, SimulatedGamePieceConstants.GamePieceType gamePieceType, - ObjectDetectionCamera camera) { + ObjectDetectionCamera... cameras) { this.deletionThresholdSeconds = deletionThresholdSeconds; this.gamePieceType = gamePieceType; - this.camera = camera; + this.cameras = cameras; this.knownObjectPositions = new HashMap<>(); this.rotationToTranslation = distanceCalculationMethod.rotationToTranslation; } @@ -151,26 +152,35 @@ private double calculateObjectDistanceRatingFromPose(Translation2d objectTransla final double translationDistance = poseTranslation.getDistance(objectTranslation); final Translation2d translationDifference = objectTranslation.minus(poseTranslation); final double rotationDifferenceDegrees = Math.abs(pose.getRotation().minus(translationDifference.getAngle()).getDegrees()); + return translationDistance * rotationToTranslation + rotationDifferenceDegrees * (1 - rotationToTranslation); } private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); - for (Translation2d visibleObject : camera.getObjectPositionsOnField(gamePieceType)) { - Translation2d closestObjectToVisibleObject = new Translation2d(); - double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; - - for (Translation2d knownObject : knownObjectPositions.keySet()) { - final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject); - if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { - closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; - closestObjectToVisibleObject = knownObject; + final Set objectsProcessedThisFrame = new HashSet<>(); + + for (ObjectDetectionCamera currentCamera : cameras) { + for (Translation2d visibleObject : currentCamera.getObjectPositionsOnField(gamePieceType)) { + Translation2d closestObjectToVisibleObject = null; + double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; + + for (Translation2d knownObject : knownObjectPositions.keySet()) { + if (!objectsProcessedThisFrame.contains(knownObject)) { + final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject); + if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { + closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; + closestObjectToVisibleObject = knownObject; + } + } } + if (closestObjectToVisibleObject != null + && closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS + && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) + removeObject(closestObjectToVisibleObject); + knownObjectPositions.put(visibleObject, currentTimestamp); + objectsProcessedThisFrame.add(visibleObject); } - - if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) - removeObject(closestObjectToVisibleObject); - knownObjectPositions.put(visibleObject, currentTimestamp); } } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java index 6fd5cdc7..3edec516 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -43,7 +43,7 @@ private PhotonPipelineResult getLatestPipelineResult() { } private void updateNoNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - inputs.seesObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + inputs.hasObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; } @@ -51,14 +51,14 @@ private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inpu final List[] visibleObjectsRotations = new List[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) visibleObjectsRotations[i] = new ArrayList<>(); - Arrays.fill(inputs.seesObject, false); + Arrays.fill(inputs.hasObject, false); inputs.latestResultTimestamp = result.getTimestampSeconds(); for (PhotonTrackedTarget currentTarget : result.getTargets()) { if (currentTarget.getDetectedObjectClassID() == -1) continue; - inputs.seesObject[currentTarget.getDetectedObjectClassID()] = true; + inputs.hasObject[currentTarget.getDetectedObjectClassID()] = true; visibleObjectsRotations[currentTarget.getDetectedObjectClassID()].add(extractRotation3d(currentTarget)); } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index bd537cbe..17bc8d76 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -35,8 +35,8 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { boolean hasAnyTarget = false; for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) { - inputs.seesObject[i] = !visibleGamePieces[i].isEmpty(); - if (inputs.seesObject[i]) + inputs.hasObject[i] = !visibleGamePieces[i].isEmpty(); + if (inputs.hasObject[i]) hasAnyTarget = true; } @@ -51,12 +51,12 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { private ArrayList>[] calculateAllVisibleGamePieces(Pose3d cameraPose) { final ArrayList>[] visibleGamePieces = new ArrayList[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; for (int i = 0; i < visibleGamePieces.length; i++) - visibleGamePieces[i] = calculateVisibleGamePiecesPositions(cameraPose, i); + visibleGamePieces[i] = calculateVisibleGamePiecesPlacement(cameraPose, i); return visibleGamePieces; } private void updateNoNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - inputs.seesObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + inputs.hasObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; } @@ -79,7 +79,7 @@ private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inpu * @param objectID the ID of the object to check for visibility * @return the placements of the visible objects, as a pair of the object and the rotation of the object relative to the camera */ - private ArrayList> calculateVisibleGamePiecesPositions(Pose3d cameraPose, int objectID) { + private ArrayList> calculateVisibleGamePiecesPlacement(Pose3d cameraPose, int objectID) { final ArrayList gamePiecesOnField = SimulationFieldHandler.getSimulatedGamePieces(); final ArrayList> visibleTargetObjects = new ArrayList<>(); for (SimulatedGamePiece currentObject : gamePiecesOnField) { From 98ab0c27a6f962e826624c047f64f0e543fbc419 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 22 Dec 2025 23:06:40 +0200 Subject: [PATCH 12/68] moved the object pose estimator to be under the pose estimator package --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 +- .../commands/commandclasses/GamePieceAutoDriveCommand.java | 2 +- .../objectdetectioncamera/ObjectDetectionCameraConstants.java | 2 +- .../poseestimator}/ObjectPoseEstimator.java | 4 +++- 4 files changed, 6 insertions(+), 4 deletions(-) rename src/main/java/frc/trigon/robot/{misc/objectdetectioncamera => poseestimation/poseestimator}/ObjectPoseEstimator.java (97%) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 5c6e9e31..bec8bdce 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -15,7 +15,7 @@ import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.constants.PathPlannerConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; +import frc.trigon.robot.poseestimation.poseestimator.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java index e0663de3..62b5e9a9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java @@ -7,7 +7,7 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; import frc.trigon.robot.constants.PathPlannerConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; +import frc.trigon.robot.poseestimation.poseestimator.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java index 35792f8b..5d4bcaa9 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java @@ -4,5 +4,5 @@ public class ObjectDetectionCameraConstants { public static final int NUMBER_OF_GAME_PIECE_TYPES = SimulatedGamePieceConstants.GamePieceType.values().length; - static final double TRACKED_OBJECT_TOLERANCE_METERS = 0.12; + public static final double TRACKED_OBJECT_TOLERANCE_METERS = 0.12; } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/ObjectPoseEstimator.java similarity index 97% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java rename to src/main/java/frc/trigon/robot/poseestimation/poseestimator/ObjectPoseEstimator.java index 0a1ca51c..d0d18e06 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/ObjectPoseEstimator.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.misc.objectdetectioncamera; +package frc.trigon.robot.poseestimation.poseestimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform2d; @@ -6,6 +6,8 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import org.littletonrobotics.junction.Logger; From fb64741435fbd7253251735dcbe417820eb4558f Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 31 Dec 2025 00:03:19 +0200 Subject: [PATCH 13/68] reorganized pose estimation packages and classes --- src/main/java/frc/trigon/robot/RobotContainer.java | 6 +++--- .../commandclasses/GamePieceAutoDriveCommand.java | 2 +- .../trigon/robot/constants/CameraConstants.java | 2 +- .../{poseestimator => }/ObjectPoseEstimator.java | 8 ++++---- .../apriltagcamera/AprilTagCamera.java | 2 +- .../ObjectDetectionCamera.java | 6 +++--- .../ObjectDetectionCameraConstants.java | 2 +- .../ObjectDetectionCameraIO.java | 2 +- .../io/PhotonObjectDetectionCameraIO.java | 8 ++++---- .../io/SimulationObjectDetectionCameraIO.java | 8 ++++---- .../RelativeRobotPoseSourceConstants.java | 2 +- .../RobotPoseEstimator.java} | 14 +++++++------- .../RobotPoseEstimatorConstants.java} | 4 ++-- .../StandardDeviations.java | 2 +- .../frc/trigon/robot/subsystems/swerve/Swerve.java | 4 ++-- .../robot/subsystems/swerve/SwerveConstants.java | 4 ++-- .../swerve/swervemodule/SwerveModule.java | 6 +++--- 17 files changed, 41 insertions(+), 41 deletions(-) rename src/main/java/frc/trigon/robot/poseestimation/{poseestimator => }/ObjectPoseEstimator.java (97%) rename src/main/java/frc/trigon/robot/{misc => poseestimation}/objectdetectioncamera/ObjectDetectionCamera.java (95%) rename src/main/java/frc/trigon/robot/{misc => poseestimation}/objectdetectioncamera/ObjectDetectionCameraConstants.java (83%) rename src/main/java/frc/trigon/robot/{misc => poseestimation}/objectdetectioncamera/ObjectDetectionCameraIO.java (94%) rename src/main/java/frc/trigon/robot/{misc => poseestimation}/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java (90%) rename src/main/java/frc/trigon/robot/{misc => poseestimation}/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java (95%) rename src/main/java/frc/trigon/robot/poseestimation/{poseestimator/PoseEstimator.java => robotposeestimator/RobotPoseEstimator.java} (94%) rename src/main/java/frc/trigon/robot/poseestimation/{poseestimator/PoseEstimatorConstants.java => robotposeestimator/RobotPoseEstimatorConstants.java} (80%) rename src/main/java/frc/trigon/robot/poseestimation/{poseestimator => robotposeestimator}/StandardDeviations.java (95%) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index bec8bdce..5f04c46c 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -15,16 +15,16 @@ import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.constants.PathPlannerConstants; -import frc.trigon.robot.poseestimation.poseestimator.ObjectPoseEstimator; +import frc.trigon.robot.poseestimation.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; import lib.utilities.flippable.Flippable; 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, diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java index 62b5e9a9..ac2ff2b1 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java @@ -7,7 +7,7 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; import frc.trigon.robot.constants.PathPlannerConstants; -import frc.trigon.robot.poseestimation.poseestimator.ObjectPoseEstimator; +import frc.trigon.robot.poseestimation.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 1b750328..777bbc2f 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/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.poseestimation.objectdetectioncamera.ObjectDetectionCamera; public class CameraConstants { private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java similarity index 97% rename from src/main/java/frc/trigon/robot/poseestimation/poseestimator/ObjectPoseEstimator.java rename to src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java index d0d18e06..378bf337 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.poseestimator; +package frc.trigon.robot.poseestimation; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform2d; @@ -6,8 +6,8 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraConstants; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import org.littletonrobotics.junction.Logger; @@ -29,7 +29,7 @@ public class ObjectPoseEstimator extends SubsystemBase { * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed * @param distanceCalculationMethod the method used to calculate the distance from the game piece * @param gamePieceType the type of game piece to track - * @param cameras the camera used for detecting objects + * @param cameras the cameras used for detecting objects */ public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, SimulatedGamePieceConstants.GamePieceType gamePieceType, diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 9a5ec54c..f44da4db 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj.DriverStation; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.poseestimation.poseestimator.StandardDeviations; +import frc.trigon.robot.poseestimation.robotposeestimator.StandardDeviations; import org.littletonrobotics.junction.Logger; /** diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCamera.java similarity index 95% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java rename to src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCamera.java index ed9c3704..c1b41183 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCamera.java @@ -1,10 +1,10 @@ -package frc.trigon.robot.misc.objectdetectioncamera; +package frc.trigon.robot.poseestimation.objectdetectioncamera; import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj2.command.SubsystemBase; 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.poseestimation.objectdetectioncamera.io.PhotonObjectDetectionCameraIO; +import frc.trigon.robot.poseestimation.objectdetectioncamera.io.SimulationObjectDetectionCameraIO; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import org.littletonrobotics.junction.Logger; import lib.hardware.RobotHardwareStats; diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraConstants.java similarity index 83% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java rename to src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraConstants.java index 5d4bcaa9..24027323 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraConstants.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.misc.objectdetectioncamera; +package frc.trigon.robot.poseestimation.objectdetectioncamera; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraIO.java similarity index 94% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraIO.java index 19ad356f..cbc2f6c0 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraIO.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.misc.objectdetectioncamera; +package frc.trigon.robot.poseestimation.objectdetectioncamera; import edu.wpi.first.math.geometry.Rotation3d; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java similarity index 90% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java index 3edec516..be0e2e21 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -1,10 +1,10 @@ -package frc.trigon.robot.misc.objectdetectioncamera.io; +package frc.trigon.robot.poseestimation.objectdetectioncamera.io; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java similarity index 95% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index 17bc8d76..31ac24ff 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -1,12 +1,12 @@ -package frc.trigon.robot.misc.objectdetectioncamera.io; +package frc.trigon.robot.poseestimation.objectdetectioncamera.io; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj.Timer; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePiece; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java index 7c7a2812..56d35893 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java @@ -1,6 +1,6 @@ package frc.trigon.robot.poseestimation.relativerobotposesource; -import frc.trigon.robot.poseestimation.poseestimator.StandardDeviations; +import frc.trigon.robot.poseestimation.robotposeestimator.StandardDeviations; public class RelativeRobotPoseSourceConstants { public static final StandardDeviations STANDARD_DEVIATIONS = new StandardDeviations(0.0000001, 0.0000001); diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java similarity index 94% rename from src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java rename to src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java index 2f430fc5..967dfbc4 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.poseestimator; +package frc.trigon.robot.poseestimation.robotposeestimator; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.VecBuilder; @@ -29,7 +29,7 @@ /** * A class that estimates the robot's pose using WPILib's {@link SwerveDrivePoseEstimator} and {@link SwerveDriveOdometry}. */ -public class PoseEstimator implements AutoCloseable { +public class RobotPoseEstimator implements AutoCloseable { private final SwerveDrivePoseEstimator swerveDrivePoseEstimator = createSwerveDrivePoseEstimator(); private final SwerveDriveOdometry swerveDriveOdometry = createSwerveDriveOdometry(); private final Field2d field = new Field2d(); @@ -44,7 +44,7 @@ public class PoseEstimator implements AutoCloseable { * @param relativeRobotPoseSource the relative robot pose source that should be used to update the pose estimator * @param aprilTagCameras the apriltag cameras that should be used to update the relative robot pose source */ - public PoseEstimator(RelativeRobotPoseSource relativeRobotPoseSource, AprilTagCamera... aprilTagCameras) { + public RobotPoseEstimator(RelativeRobotPoseSource relativeRobotPoseSource, AprilTagCamera... aprilTagCameras) { this.aprilTagCameras = aprilTagCameras; this.relativeRobotPoseSource = relativeRobotPoseSource; this.shouldUseRelativeRobotPoseSource = true; @@ -58,7 +58,7 @@ public PoseEstimator(RelativeRobotPoseSource relativeRobotPoseSource, AprilTagCa * * @param aprilTagCameras the cameras that should be used to update the pose estimator */ - public PoseEstimator(AprilTagCamera... aprilTagCameras) { + public RobotPoseEstimator(AprilTagCamera... aprilTagCameras) { this.aprilTagCameras = aprilTagCameras; this.relativeRobotPoseSource = null; this.shouldUseRelativeRobotPoseSource = false; @@ -229,8 +229,8 @@ private boolean isUnderMaximumSpeedForOffsetResetting() { final ChassisSpeeds chassisSpeeds = RobotContainer.SWERVE.getSelfRelativeVelocity(); final double currentTranslationVelocityMetersPerSecond = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); final double currentThetaVelocityRadiansPerSecond = chassisSpeeds.omegaRadiansPerSecond; - return currentTranslationVelocityMetersPerSecond <= PoseEstimatorConstants.MAXIMUM_TRANSLATION_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_METERS_PER_SECOND && - currentThetaVelocityRadiansPerSecond <= PoseEstimatorConstants.MAXIMUM_THETA_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_RADIANS_PER_SECOND; + return currentTranslationVelocityMetersPerSecond <= RobotPoseEstimatorConstants.MAXIMUM_TRANSLATION_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_METERS_PER_SECOND && + currentThetaVelocityRadiansPerSecond <= RobotPoseEstimatorConstants.MAXIMUM_THETA_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_RADIANS_PER_SECOND; } private AprilTagCamera[] getCamerasWithResults() { @@ -280,7 +280,7 @@ private SwerveDrivePoseEstimator createSwerveDrivePoseEstimator() { new Rotation2d(), swerveModulePositions, new Pose2d(), - PoseEstimatorConstants.ODOMETRY_STANDARD_DEVIATIONS.toMatrix(), + RobotPoseEstimatorConstants.ODOMETRY_STANDARD_DEVIATIONS.toMatrix(), VecBuilder.fill(0, 0, 0) ); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimatorConstants.java similarity index 80% rename from src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java rename to src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimatorConstants.java index d341b294..74be9b54 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimatorConstants.java @@ -1,6 +1,6 @@ -package frc.trigon.robot.poseestimation.poseestimator; +package frc.trigon.robot.poseestimation.robotposeestimator; -public class PoseEstimatorConstants { +public class RobotPoseEstimatorConstants { public static final double ODOMETRY_FREQUENCY_HERTZ = 250; static final StandardDeviations ODOMETRY_STANDARD_DEVIATIONS = new StandardDeviations(0.003, 0.0002); diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/StandardDeviations.java similarity index 95% rename from src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java rename to src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/StandardDeviations.java index bbe11c6c..689e45c0 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/StandardDeviations.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.poseestimator; +package frc.trigon.robot.poseestimation.robotposeestimator; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 5fc7d07e..9e9ce335 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.PathPlannerConstants; -import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants; @@ -34,7 +34,7 @@ public class Swerve extends MotorSubsystem { public Swerve() { setName("Swerve"); - phoenix6SignalThread.setThreadFrequencyHertz(PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + phoenix6SignalThread.setThreadFrequencyHertz(RobotPoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index ab0dbbdf..a46ffb81 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -10,7 +10,7 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.constants.RobotConstants; -import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimatorConstants; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; import lib.hardware.RobotHardwareStats; import lib.hardware.phoenix6.pigeon2.Pigeon2Gyro; @@ -107,6 +107,6 @@ private static void configureGyro() { GYRO.applyConfiguration(config); GYRO.setSimulationYawVelocitySupplier(() -> RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond); - GYRO.registerThreadedSignal(Pigeon2Signal.YAW, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + GYRO.registerThreadedSignal(Pigeon2Signal.YAW, RobotPoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java index f53c9a6c..cb38a539 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java @@ -9,7 +9,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import frc.trigon.robot.constants.RobotConstants; -import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimatorConstants; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; @@ -200,7 +200,7 @@ private void configureDriveMotor() { driveMotor.registerSignal(TalonFXSignal.VELOCITY, 100); driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, RobotPoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } private void configureSteerMotor() { @@ -209,7 +209,7 @@ private void configureSteerMotor() { steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, RobotPoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } private void configureSteerEncoder(double offsetRotations) { From 78cefa21790962cd22077221148c2704539c5e4e Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 1 Jan 2026 12:33:30 +0200 Subject: [PATCH 14/68] reformatted --- .../java/frc/trigon/robot/constants/CameraConstants.java | 2 +- .../trigon/robot/poseestimation/ObjectPoseEstimator.java | 4 ++-- .../{ => cameras}/apriltagcamera/AprilTagCamera.java | 2 +- .../apriltagcamera/AprilTagCameraConstants.java | 8 ++++---- .../{ => cameras}/apriltagcamera/AprilTagCameraIO.java | 2 +- .../apriltagcamera/io/AprilTagLimelightIO.java | 6 +++--- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 8 ++++---- .../apriltagcamera/io/AprilTagSimulationCameraIO.java | 4 ++-- .../objectdetectioncamera/ObjectDetectionCamera.java | 6 +++--- .../ObjectDetectionCameraConstants.java | 2 +- .../objectdetectioncamera/ObjectDetectionCameraIO.java | 2 +- .../io/PhotonObjectDetectionCameraIO.java | 8 ++++---- .../io/SimulationObjectDetectionCameraIO.java | 8 ++++---- .../robotposeestimator/RobotPoseEstimator.java | 2 +- 14 files changed, 32 insertions(+), 32 deletions(-) rename src/main/java/frc/trigon/robot/poseestimation/{ => cameras}/apriltagcamera/AprilTagCamera.java (99%) rename src/main/java/frc/trigon/robot/poseestimation/{ => cameras}/apriltagcamera/AprilTagCameraConstants.java (90%) rename src/main/java/frc/trigon/robot/poseestimation/{ => cameras}/apriltagcamera/AprilTagCameraIO.java (95%) rename src/main/java/frc/trigon/robot/poseestimation/{ => cameras}/apriltagcamera/io/AprilTagLimelightIO.java (90%) rename src/main/java/frc/trigon/robot/poseestimation/{ => cameras}/apriltagcamera/io/AprilTagPhotonCameraIO.java (95%) rename src/main/java/frc/trigon/robot/poseestimation/{ => cameras}/apriltagcamera/io/AprilTagSimulationCameraIO.java (79%) rename src/main/java/frc/trigon/robot/poseestimation/{ => cameras}/objectdetectioncamera/ObjectDetectionCamera.java (95%) rename src/main/java/frc/trigon/robot/poseestimation/{ => cameras}/objectdetectioncamera/ObjectDetectionCameraConstants.java (81%) rename src/main/java/frc/trigon/robot/poseestimation/{ => cameras}/objectdetectioncamera/ObjectDetectionCameraIO.java (93%) rename src/main/java/frc/trigon/robot/poseestimation/{ => cameras}/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java (89%) rename src/main/java/frc/trigon/robot/poseestimation/{ => cameras}/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java (94%) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 777bbc2f..1e1726b1 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/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.poseestimation.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCamera; public class CameraConstants { private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( diff --git a/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java index 378bf337..176b1a6a 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java @@ -6,8 +6,8 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCamera; -import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraConstants; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCamera.java similarity index 99% rename from src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java rename to src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCamera.java index f44da4db..7f81e99f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCamera.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.apriltagcamera; +package frc.trigon.robot.poseestimation.cameras.apriltagcamera; import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj.DriverStation; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraConstants.java similarity index 90% rename from src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java rename to src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraConstants.java index 8569344c..ef556db7 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraConstants.java @@ -1,12 +1,12 @@ -package frc.trigon.robot.poseestimation.apriltagcamera; +package frc.trigon.robot.poseestimation.cameras.apriltagcamera; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; -import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; -import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagSimulationCameraIO; +import frc.trigon.robot.poseestimation.cameras.apriltagcamera.io.AprilTagLimelightIO; +import frc.trigon.robot.poseestimation.cameras.apriltagcamera.io.AprilTagPhotonCameraIO; +import frc.trigon.robot.poseestimation.cameras.apriltagcamera.io.AprilTagSimulationCameraIO; import org.photonvision.PhotonPoseEstimator; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraIO.java similarity index 95% rename from src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraIO.java index 31dcd576..e2709dd2 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraIO.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.apriltagcamera; +package frc.trigon.robot.poseestimation.cameras.apriltagcamera; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagLimelightIO.java similarity index 90% rename from src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java rename to src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagLimelightIO.java index 60a0e81b..8ac74388 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagLimelightIO.java @@ -1,9 +1,9 @@ -package frc.trigon.robot.poseestimation.apriltagcamera.io; +package frc.trigon.robot.poseestimation.cameras.apriltagcamera.io; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraInputsAutoLogged; import frc.trigon.lib.utilities.LimelightHelpers; // TODO: Fully implement this class, Limelight currently not supported. diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagPhotonCameraIO.java similarity index 95% rename from src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagPhotonCameraIO.java index a8e2bfba..1554217b 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.apriltagcamera.io; +package frc.trigon.robot.poseestimation.cameras.apriltagcamera.io; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; @@ -10,9 +10,9 @@ import edu.wpi.first.math.numbers.N8; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; +import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraInputsAutoLogged; import org.photonvision.PhotonCamera; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagSimulationCameraIO.java similarity index 79% rename from src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagSimulationCameraIO.java index c68ef0e0..a80a70ae 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagSimulationCameraIO.java @@ -1,7 +1,7 @@ -package frc.trigon.robot.poseestimation.apriltagcamera.io; +package frc.trigon.robot.poseestimation.cameras.apriltagcamera.io; import edu.wpi.first.math.geometry.Transform3d; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraConstants; import org.photonvision.simulation.PhotonCameraSim; public class AprilTagSimulationCameraIO extends AprilTagPhotonCameraIO { diff --git a/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCamera.java similarity index 95% rename from src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCamera.java rename to src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCamera.java index fa4066e8..a13c03c8 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCamera.java @@ -1,10 +1,10 @@ -package frc.trigon.robot.poseestimation.objectdetectioncamera; +package frc.trigon.robot.poseestimation.cameras.objectdetectioncamera; import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.poseestimation.objectdetectioncamera.io.PhotonObjectDetectionCameraIO; -import frc.trigon.robot.poseestimation.objectdetectioncamera.io.SimulationObjectDetectionCameraIO; +import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.io.PhotonObjectDetectionCameraIO; +import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.io.SimulationObjectDetectionCameraIO; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import org.littletonrobotics.junction.Logger; import frc.trigon.lib.hardware.RobotHardwareStats; diff --git a/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraConstants.java similarity index 81% rename from src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraConstants.java rename to src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraConstants.java index 24027323..c8ad398c 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraConstants.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.objectdetectioncamera; +package frc.trigon.robot.poseestimation.cameras.objectdetectioncamera; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; diff --git a/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraIO.java similarity index 93% rename from src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraIO.java index cbc2f6c0..cea849d7 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/ObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraIO.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.objectdetectioncamera; +package frc.trigon.robot.poseestimation.cameras.objectdetectioncamera; import edu.wpi.first.math.geometry.Rotation3d; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java similarity index 89% rename from src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java index be0e2e21..a7956c85 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -1,10 +1,10 @@ -package frc.trigon.robot.poseestimation.objectdetectioncamera.io; +package frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.io; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraConstants; -import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraIO; -import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; diff --git a/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java similarity index 94% rename from src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index 31ac24ff..f088540a 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -1,12 +1,12 @@ -package frc.trigon.robot.poseestimation.objectdetectioncamera.io; +package frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.io; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj.Timer; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraConstants; -import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraIO; -import frc.trigon.robot.poseestimation.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePiece; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java index cd3f86d5..838b14be 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java @@ -14,7 +14,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCamera; import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSource; import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceConstants; import frc.trigon.robot.subsystems.swerve.SwerveConstants; From 295ebe85b80766ab2fe6c382a80209ed888e5532 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 1 Jan 2026 12:55:53 +0200 Subject: [PATCH 15/68] extracted update object positions into a few functions to make more readable --- .../poseestimation/ObjectPoseEstimator.java | 49 ++++++++++++------- 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java index 176b1a6a..6638e395 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java @@ -6,9 +6,9 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraConstants; -import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -158,34 +158,49 @@ private double calculateObjectDistanceRatingFromPose(Translation2d objectTransla return translationDistance * rotationToTranslation + rotationDifferenceDegrees * (1 - rotationToTranslation); } + private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); final Set objectsProcessedThisFrame = new HashSet<>(); for (ObjectDetectionCamera currentCamera : cameras) { for (Translation2d visibleObject : currentCamera.getObjectPositionsOnField(gamePieceType)) { - Translation2d closestObjectToVisibleObject = null; - double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; - - for (Translation2d knownObject : knownObjectPositions.keySet()) { - if (!objectsProcessedThisFrame.contains(knownObject)) { - final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject); - if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { - closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; - closestObjectToVisibleObject = knownObject; - } - } - } - if (closestObjectToVisibleObject != null - && closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS - && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) - removeObject(closestObjectToVisibleObject); + Translation2d closestKnownObjectToVisibleObject = getClosestKnownObjectToVisibleObject(visibleObject, objectsProcessedThisFrame); + double closestKnownObjectToVisibleObjectDistanceMeters = closestKnownObjectToVisibleObject != null + ? visibleObject.getDistance(closestKnownObjectToVisibleObject) + : Double.POSITIVE_INFINITY; + + if (shouldReplaceObject(closestKnownObjectToVisibleObject, closestKnownObjectToVisibleObjectDistanceMeters, currentTimestamp)) + removeObject(closestKnownObjectToVisibleObject); + knownObjectPositions.put(visibleObject, currentTimestamp); objectsProcessedThisFrame.add(visibleObject); } } } + private boolean shouldReplaceObject(Translation2d object, double closestObjectToVisibleDistanceMeters, double currentTimestamp) { + return object != null + && closestObjectToVisibleDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS + && knownObjectPositions.get(object) != currentTimestamp; + } + + private Translation2d getClosestKnownObjectToVisibleObject(Translation2d visibleObject, Set processedObjects) { + Translation2d closestObjectToVisibleObject = null; + double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; + + for (Translation2d currentObject : knownObjectPositions.keySet()) { + if (!processedObjects.contains(currentObject)) { + final double currentObjectDistanceMeters = visibleObject.getDistance(currentObject); + if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { + closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; + closestObjectToVisibleObject = currentObject; + } + } + } + return closestObjectToVisibleObject; + } + private void removeOldObjects() { knownObjectPositions.entrySet().removeIf(entry -> isTooOld(entry.getValue())); } From fff101a6bf12a08933229e0aa1124a1974b06541 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 1 Jan 2026 13:07:12 +0200 Subject: [PATCH 16/68] fixed incorrect condition --- .../frc/trigon/robot/poseestimation/ObjectPoseEstimator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java index 6638e395..445fbdde 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java @@ -182,7 +182,7 @@ private void updateObjectPositions() { private boolean shouldReplaceObject(Translation2d object, double closestObjectToVisibleDistanceMeters, double currentTimestamp) { return object != null && closestObjectToVisibleDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS - && knownObjectPositions.get(object) != currentTimestamp; + && knownObjectPositions.get(object).equals(currentTimestamp); } private Translation2d getClosestKnownObjectToVisibleObject(Translation2d visibleObject, Set processedObjects) { From 45ecfa2927d0845e8bba907e0566185195438e18 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 1 Jan 2026 13:10:14 +0200 Subject: [PATCH 17/68] java doc fix --- .../poseestimation/robotposeestimator/RobotPoseEstimator.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java index 838b14be..8699cc34 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java @@ -38,7 +38,7 @@ public class RobotPoseEstimator implements AutoCloseable { private final boolean shouldUseRelativeRobotPoseSource; /** - * Constructs a new PoseEstimator and sets the relativeRobotPoseSource. + * Constructs a new RobotPoseEstimator and sets the relativeRobotPoseSource. * This constructor enables usage of a relative robot pose source and disables the use of april tags for pose estimation, and instead uses them to reset the relative robot pose source's offset. * * @param relativeRobotPoseSource the relative robot pose source that should be used to update the pose estimator @@ -53,7 +53,7 @@ public RobotPoseEstimator(RelativeRobotPoseSource relativeRobotPoseSource, April } /** - * Constructs a new PoseEstimator. + * Constructs a new RobotPoseEstimator. * This constructor disables the use of a relative robot pose source and instead uses april tags cameras for pose estimation. * * @param aprilTagCameras the cameras that should be used to update the pose estimator From ece1bda80541a72d30792c3aac7f921c34326b85 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 1 Jan 2026 13:27:10 +0200 Subject: [PATCH 18/68] update logic issue --- .../trigon/robot/poseestimation/ObjectPoseEstimator.java | 8 ++++---- .../objectdetectioncamera/ObjectDetectionCamera.java | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java index 445fbdde..239a554c 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java @@ -161,11 +161,11 @@ private double calculateObjectDistanceRatingFromPose(Translation2d objectTransla private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); - final Set objectsProcessedThisFrame = new HashSet<>(); + final Set objectsProcessed = new HashSet<>(); for (ObjectDetectionCamera currentCamera : cameras) { for (Translation2d visibleObject : currentCamera.getObjectPositionsOnField(gamePieceType)) { - Translation2d closestKnownObjectToVisibleObject = getClosestKnownObjectToVisibleObject(visibleObject, objectsProcessedThisFrame); + Translation2d closestKnownObjectToVisibleObject = getClosestKnownObjectToVisibleObject(visibleObject, objectsProcessed); double closestKnownObjectToVisibleObjectDistanceMeters = closestKnownObjectToVisibleObject != null ? visibleObject.getDistance(closestKnownObjectToVisibleObject) : Double.POSITIVE_INFINITY; @@ -174,7 +174,7 @@ private void updateObjectPositions() { removeObject(closestKnownObjectToVisibleObject); knownObjectPositions.put(visibleObject, currentTimestamp); - objectsProcessedThisFrame.add(visibleObject); + objectsProcessed.add(visibleObject); } } } @@ -182,7 +182,7 @@ private void updateObjectPositions() { private boolean shouldReplaceObject(Translation2d object, double closestObjectToVisibleDistanceMeters, double currentTimestamp) { return object != null && closestObjectToVisibleDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS - && knownObjectPositions.get(object).equals(currentTimestamp); + && !knownObjectPositions.get(object).equals(currentTimestamp); } private Translation2d getClosestKnownObjectToVisibleObject(Translation2d visibleObject, Set processedObjects) { diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCamera.java index a13c03c8..cfb7db3a 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCamera.java @@ -2,12 +2,12 @@ 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.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.io.PhotonObjectDetectionCameraIO; import frc.trigon.robot.poseestimation.cameras.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. From 251248e897584e800405c5ef850c4e1803bea046 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 5 Jan 2026 12:59:11 +0200 Subject: [PATCH 19/68] reorganized obejct pose estimation to what it was before --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 +- .../commandclasses/GamePieceAutoDriveCommand.java | 2 +- .../java/frc/trigon/robot/constants/CameraConstants.java | 2 +- .../{poseestimation => misc}/ObjectPoseEstimator.java | 6 +++--- .../objectdetectioncamera/ObjectDetectionCamera.java | 6 +++--- .../ObjectDetectionCameraConstants.java | 2 +- .../objectdetectioncamera/ObjectDetectionCameraIO.java | 2 +- .../io/PhotonObjectDetectionCameraIO.java | 8 ++++---- .../io/SimulationObjectDetectionCameraIO.java | 8 ++++---- .../{cameras => }/apriltagcamera/AprilTagCamera.java | 2 +- .../apriltagcamera/AprilTagCameraConstants.java | 8 ++++---- .../{cameras => }/apriltagcamera/AprilTagCameraIO.java | 2 +- .../apriltagcamera/io/AprilTagLimelightIO.java | 6 +++--- .../apriltagcamera/io/AprilTagPhotonCameraIO.java | 8 ++++---- .../apriltagcamera/io/AprilTagSimulationCameraIO.java | 4 ++-- .../robotposeestimator/RobotPoseEstimator.java | 2 +- 16 files changed, 35 insertions(+), 35 deletions(-) rename src/main/java/frc/trigon/robot/{poseestimation => misc}/ObjectPoseEstimator.java (97%) rename src/main/java/frc/trigon/robot/{poseestimation/cameras => misc}/objectdetectioncamera/ObjectDetectionCamera.java (95%) rename src/main/java/frc/trigon/robot/{poseestimation/cameras => misc}/objectdetectioncamera/ObjectDetectionCameraConstants.java (81%) rename src/main/java/frc/trigon/robot/{poseestimation/cameras => misc}/objectdetectioncamera/ObjectDetectionCameraIO.java (93%) rename src/main/java/frc/trigon/robot/{poseestimation/cameras => misc}/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java (89%) rename src/main/java/frc/trigon/robot/{poseestimation/cameras => misc}/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java (94%) rename src/main/java/frc/trigon/robot/poseestimation/{cameras => }/apriltagcamera/AprilTagCamera.java (99%) rename src/main/java/frc/trigon/robot/poseestimation/{cameras => }/apriltagcamera/AprilTagCameraConstants.java (90%) rename src/main/java/frc/trigon/robot/poseestimation/{cameras => }/apriltagcamera/AprilTagCameraIO.java (95%) rename src/main/java/frc/trigon/robot/poseestimation/{cameras => }/apriltagcamera/io/AprilTagLimelightIO.java (90%) rename src/main/java/frc/trigon/robot/poseestimation/{cameras => }/apriltagcamera/io/AprilTagPhotonCameraIO.java (95%) rename src/main/java/frc/trigon/robot/poseestimation/{cameras => }/apriltagcamera/io/AprilTagSimulationCameraIO.java (79%) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 4203f3dd..9ce869b9 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -16,7 +16,7 @@ import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.poseestimation.ObjectPoseEstimator; +import frc.trigon.robot.misc.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java index 59c0815e..de0ab8ea 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java @@ -7,7 +7,7 @@ import frc.trigon.lib.utilities.flippable.FlippableRotation2d; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; -import frc.trigon.robot.poseestimation.ObjectPoseEstimator; +import frc.trigon.robot.misc.ObjectPoseEstimator; import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 1e1726b1..1b750328 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/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.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; public class CameraConstants { private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( diff --git a/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/ObjectPoseEstimator.java similarity index 97% rename from src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java rename to src/main/java/frc/trigon/robot/misc/ObjectPoseEstimator.java index 239a554c..4896e22d 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/ObjectPoseEstimator.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation; +package frc.trigon.robot.misc; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform2d; @@ -7,8 +7,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; -import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCamera; -import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java similarity index 95% rename from src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCamera.java rename to src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java index cfb7db3a..6239d9f5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java @@ -1,12 +1,12 @@ -package frc.trigon.robot.poseestimation.cameras.objectdetectioncamera; +package frc.trigon.robot.misc.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.simulatedfield.SimulatedGamePieceConstants; -import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.io.PhotonObjectDetectionCameraIO; -import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.io.SimulationObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectdetectioncamera.io.PhotonObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectdetectioncamera.io.SimulationObjectDetectionCameraIO; import org.littletonrobotics.junction.Logger; /** diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraConstants.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java similarity index 81% rename from src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraConstants.java rename to src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java index c8ad398c..5d4bcaa9 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraConstants.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.cameras.objectdetectioncamera; +package frc.trigon.robot.misc.objectdetectioncamera; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java similarity index 93% rename from src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java index cea849d7..19ad356f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/ObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.cameras.objectdetectioncamera; +package frc.trigon.robot.misc.objectdetectioncamera; import edu.wpi.first.math.geometry.Rotation3d; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java similarity index 89% rename from src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java index a7956c85..3edec516 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -1,10 +1,10 @@ -package frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.io; +package frc.trigon.robot.misc.objectdetectioncamera.io; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraConstants; -import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraIO; -import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java similarity index 94% rename from src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index f088540a..17bc8d76 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -1,12 +1,12 @@ -package frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.io; +package frc.trigon.robot.misc.objectdetectioncamera.io; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj.Timer; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraConstants; -import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraIO; -import frc.trigon.robot.poseestimation.cameras.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePiece; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java similarity index 99% rename from src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCamera.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 7f81e99f..f44da4db 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.cameras.apriltagcamera; +package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj.DriverStation; diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java similarity index 90% rename from src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraConstants.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index ef556db7..8569344c 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -1,12 +1,12 @@ -package frc.trigon.robot.poseestimation.cameras.apriltagcamera; +package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.poseestimation.cameras.apriltagcamera.io.AprilTagLimelightIO; -import frc.trigon.robot.poseestimation.cameras.apriltagcamera.io.AprilTagPhotonCameraIO; -import frc.trigon.robot.poseestimation.cameras.apriltagcamera.io.AprilTagSimulationCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagSimulationCameraIO; import org.photonvision.PhotonPoseEstimator; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java similarity index 95% rename from src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index e2709dd2..31dcd576 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.cameras.apriltagcamera; +package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java similarity index 90% rename from src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagLimelightIO.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 8ac74388..60a0e81b 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -1,9 +1,9 @@ -package frc.trigon.robot.poseestimation.cameras.apriltagcamera.io; +package frc.trigon.robot.poseestimation.apriltagcamera.io; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; -import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraIO; -import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraInputsAutoLogged; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; import frc.trigon.lib.utilities.LimelightHelpers; // TODO: Fully implement this class, Limelight currently not supported. diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java similarity index 95% rename from src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagPhotonCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index 1554217b..a8e2bfba 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.cameras.apriltagcamera.io; +package frc.trigon.robot.poseestimation.apriltagcamera.io; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose3d; @@ -10,9 +10,9 @@ import edu.wpi.first.math.numbers.N8; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraConstants; -import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraIO; -import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraInputsAutoLogged; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; import org.photonvision.PhotonCamera; import org.photonvision.estimation.TargetModel; import org.photonvision.estimation.VisionEstimation; diff --git a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagSimulationCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java similarity index 79% rename from src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagSimulationCameraIO.java rename to src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java index a80a70ae..c68ef0e0 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/cameras/apriltagcamera/io/AprilTagSimulationCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java @@ -1,7 +1,7 @@ -package frc.trigon.robot.poseestimation.cameras.apriltagcamera.io; +package frc.trigon.robot.poseestimation.apriltagcamera.io; import edu.wpi.first.math.geometry.Transform3d; -import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; import org.photonvision.simulation.PhotonCameraSim; public class AprilTagSimulationCameraIO extends AprilTagPhotonCameraIO { diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java index 8699cc34..9e6da86f 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java @@ -14,7 +14,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.poseestimation.cameras.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSource; import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceConstants; import frc.trigon.robot.subsystems.swerve.SwerveConstants; From b131bcd76cd8d57be37fe3b81e39b2ab64bd225d Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 5 Jan 2026 13:10:37 +0200 Subject: [PATCH 20/68] Update lib --- src/main/java/frc/trigon/lib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 553e7916..77c1f252 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 553e7916e7607dde051530e8903879fb590e573c +Subproject commit 77c1f2526dfb1dc1d53842927a70e48ead1cf20b From e5d38b01fd19f4c98de29c0ab528f6de8bfec13a Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 6 Jan 2026 18:26:53 +0200 Subject: [PATCH 21/68] renaming and organizing --- .../java/frc/trigon/robot/RobotContainer.java | 2 +- .../GamePieceAutoDriveCommand.java | 2 +- .../robot/constants/CameraConstants.java | 2 +- .../ObjectDetectionConstants.java} | 4 ++-- .../ObjectPoseEstimator.java | 7 +++--- .../ObjectDetectionCamera.java | 6 ++--- .../ObjectDetectionCameraIO.java | 7 +++--- .../io/PhotonObjectDetectionCameraIO.java | 18 +++++++-------- .../io/SimulationObjectDetectionCameraIO.java | 22 +++++++++---------- 9 files changed, 35 insertions(+), 35 deletions(-) rename src/main/java/frc/trigon/robot/misc/{objectdetectioncamera/ObjectDetectionCameraConstants.java => objectDetection/ObjectDetectionConstants.java} (72%) rename src/main/java/frc/trigon/robot/misc/{ => objectDetection}/ObjectPoseEstimator.java (97%) rename src/main/java/frc/trigon/robot/misc/{ => objectDetection}/objectdetectioncamera/ObjectDetectionCamera.java (95%) rename src/main/java/frc/trigon/robot/misc/{ => objectDetection}/objectdetectioncamera/ObjectDetectionCameraIO.java (78%) rename src/main/java/frc/trigon/robot/misc/{ => objectDetection}/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java (79%) rename src/main/java/frc/trigon/robot/misc/{ => objectDetection}/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java (88%) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 3204f993..839a3ae3 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -16,7 +16,7 @@ import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.misc.ObjectPoseEstimator; +import frc.trigon.robot.misc.objectDetection.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java index fa81959d..79abd00c 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java @@ -7,7 +7,7 @@ import frc.trigon.lib.utilities.flippable.FlippableRotation2d; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; -import frc.trigon.robot.misc.ObjectPoseEstimator; +import frc.trigon.robot.misc.objectDetection.ObjectPoseEstimator; import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 1b750328..283daf96 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/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( diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java similarity index 72% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java rename to src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java index 5d4bcaa9..aa4e647c 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java +++ b/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; public static final double TRACKED_OBJECT_TOLERANCE_METERS = 0.12; } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java similarity index 97% rename from src/main/java/frc/trigon/robot/misc/ObjectPoseEstimator.java rename to src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 4896e22d..c155d1e6 100644 --- a/src/main/java/frc/trigon/robot/misc/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.misc; +package frc.trigon.robot.misc.objectDetection; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform2d; @@ -7,8 +7,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCamera; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -181,7 +180,7 @@ private void updateObjectPositions() { private boolean shouldReplaceObject(Translation2d object, double closestObjectToVisibleDistanceMeters, double currentTimestamp) { return object != null - && closestObjectToVisibleDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS + && closestObjectToVisibleDistanceMeters < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS && !knownObjectPositions.get(object).equals(currentTimestamp); } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java similarity index 95% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java rename to src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java index 6239d9f5..2a8de8e5 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java @@ -1,12 +1,12 @@ -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.simulatedfield.SimulatedGamePieceConstants; -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 org.littletonrobotics.junction.Logger; /** diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCameraIO.java similarity index 78% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCameraIO.java index 19ad356f..251bc164 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCameraIO.java @@ -1,6 +1,7 @@ -package frc.trigon.robot.misc.objectdetectioncamera; +package frc.trigon.robot.misc.objectDetection.objectdetectioncamera; import edu.wpi.first.math.geometry.Rotation3d; +import frc.trigon.robot.misc.objectDetection.ObjectDetectionConstants; import org.littletonrobotics.junction.AutoLog; public class ObjectDetectionCameraIO { @@ -15,13 +16,13 @@ public static class ObjectDetectionCameraInputs { /** * Whether there is at least one object or not for each game piece, by game piece index (type). */ - public boolean[] hasObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + public boolean[] hasObject = new boolean[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; /** * Stores the Rotation3d of all visible objects. * The first index is the game piece ID (type). * The second index is the index of the game piece's Rotation3d, with the best object placed first (index 0). */ - public Rotation3d[][] visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; + public Rotation3d[][] visibleObjectRotations = new Rotation3d[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; public double latestResultTimestamp = 0; } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java similarity index 79% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java index 3edec516..1922cf41 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -1,10 +1,10 @@ -package frc.trigon.robot.misc.objectdetectioncamera.io; +package frc.trigon.robot.misc.objectDetection.objectdetectioncamera.io; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import frc.trigon.robot.misc.objectDetection.ObjectDetectionConstants; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -43,13 +43,13 @@ private PhotonPipelineResult getLatestPipelineResult() { } private void updateNoNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - inputs.hasObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; - inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; + inputs.hasObject = new boolean[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; + inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; } private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs, PhotonPipelineResult result) { - final List[] visibleObjectsRotations = new List[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; - for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) + final List[] visibleObjectsRotations = new List[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; + for (int i = 0; i < ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) visibleObjectsRotations[i] = new ArrayList<>(); Arrays.fill(inputs.hasObject, false); inputs.latestResultTimestamp = result.getTimestampSeconds(); @@ -62,7 +62,7 @@ private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inpu visibleObjectsRotations[currentTarget.getDetectedObjectClassID()].add(extractRotation3d(currentTarget)); } - for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) + for (int i = 0; i < ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) inputs.visibleObjectRotations[i] = toArray(visibleObjectsRotations[i]); } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java similarity index 88% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index 17bc8d76..1312307e 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -1,12 +1,12 @@ -package frc.trigon.robot.misc.objectdetectioncamera.io; +package frc.trigon.robot.misc.objectDetection.objectdetectioncamera.io; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj.Timer; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import frc.trigon.robot.misc.objectDetection.ObjectDetectionConstants; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePiece; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; @@ -34,7 +34,7 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { final ArrayList>[] visibleGamePieces = calculateAllVisibleGamePieces(cameraPose); boolean hasAnyTarget = false; - for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) { + for (int i = 0; i < ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) { inputs.hasObject[i] = !visibleGamePieces[i].isEmpty(); if (inputs.hasObject[i]) hasAnyTarget = true; @@ -49,15 +49,15 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { } private ArrayList>[] calculateAllVisibleGamePieces(Pose3d cameraPose) { - final ArrayList>[] visibleGamePieces = new ArrayList[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + final ArrayList>[] visibleGamePieces = new ArrayList[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; for (int i = 0; i < visibleGamePieces.length; i++) visibleGamePieces[i] = calculateVisibleGamePiecesPlacement(cameraPose, i); return visibleGamePieces; } private void updateNoNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - inputs.hasObject = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; - inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; + inputs.hasObject = new boolean[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; + inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; } private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs, ArrayList>[] visibleGamePieces) { @@ -81,17 +81,17 @@ private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inpu */ private ArrayList> calculateVisibleGamePiecesPlacement(Pose3d cameraPose, int objectID) { final ArrayList gamePiecesOnField = SimulationFieldHandler.getSimulatedGamePieces(); - final ArrayList> visibleTargetObjects = new ArrayList<>(); + final ArrayList> visibleObjects = new ArrayList<>(); for (SimulatedGamePiece currentObject : gamePiecesOnField) { if (currentObject.isScored()) continue; final Rotation3d cameraAngleToObject = calculateCameraAngleToObject(currentObject.getPose(), cameraPose); if (isObjectWithinFOV(cameraAngleToObject)) - visibleTargetObjects.add(new Pair<>(currentObject, cameraAngleToObject)); + visibleObjects.add(new Pair<>(currentObject, cameraAngleToObject)); } - return visibleTargetObjects; + return visibleObjects; } private Rotation3d calculateCameraAngleToObject(Pose3d objectPose, Pose3d cameraPose) { From ffe948efe4565445e666894e7903e38c457bcca3 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 6 Jan 2026 18:33:17 +0200 Subject: [PATCH 22/68] rename --- .../io/SimulationObjectDetectionCameraIO.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index 1312307e..22f1b8f7 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -33,14 +33,14 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { final Pose3d cameraPose = new Pose3d(robotPose).plus(robotCenterToCamera); final ArrayList>[] visibleGamePieces = calculateAllVisibleGamePieces(cameraPose); - boolean hasAnyTarget = false; + boolean hasAnyObject = false; for (int i = 0; i < ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) { inputs.hasObject[i] = !visibleGamePieces[i].isEmpty(); if (inputs.hasObject[i]) - hasAnyTarget = true; + hasAnyObject = true; } - if (hasAnyTarget) { + if (hasAnyObject) { updateHasNewResultInputs(inputs, visibleGamePieces); return; } @@ -51,7 +51,7 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { private ArrayList>[] calculateAllVisibleGamePieces(Pose3d cameraPose) { final ArrayList>[] visibleGamePieces = new ArrayList[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; for (int i = 0; i < visibleGamePieces.length; i++) - visibleGamePieces[i] = calculateVisibleGamePiecesPlacement(cameraPose, i); + visibleGamePieces[i] = calculateVisibleGamePiecesRotations(cameraPose, i); return visibleGamePieces; } @@ -79,7 +79,7 @@ private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inpu * @param objectID the ID of the object to check for visibility * @return the placements of the visible objects, as a pair of the object and the rotation of the object relative to the camera */ - private ArrayList> calculateVisibleGamePiecesPlacement(Pose3d cameraPose, int objectID) { + private ArrayList> calculateVisibleGamePiecesRotations(Pose3d cameraPose, int objectID) { final ArrayList gamePiecesOnField = SimulationFieldHandler.getSimulatedGamePieces(); final ArrayList> visibleObjects = new ArrayList<>(); for (SimulatedGamePiece currentObject : gamePiecesOnField) { From a57077605865a2c4bcc2311a0eeeb584e96508a9 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 6 Jan 2026 18:33:55 +0200 Subject: [PATCH 23/68] removed distance rating --- .../objectDetection/ObjectPoseEstimator.java | 21 +------------------ 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index c155d1e6..be5702c6 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -6,8 +6,8 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -139,25 +139,6 @@ public Translation2d getClosestObjectToPosition(Translation2d position) { return closestObjectTranslation; } - /** - * Calculates the "distance rating" of an object from a given pose. - * The "distance rating" is a unit used to calculate the distance between 2 poses. - * It factors in both translation and rotation differences by scaling the units depending on the {@link DistanceCalculationMethod}. - * - * @param objectTranslation the translation of the object on the field - * @param pose the pose to which the distance is measured from - * @return the objects "distance rating" from the given pose - */ - private double calculateObjectDistanceRatingFromPose(Translation2d objectTranslation, Pose2d pose) { - final Translation2d poseTranslation = pose.getTranslation(); - final double translationDistance = poseTranslation.getDistance(objectTranslation); - final Translation2d translationDifference = objectTranslation.minus(poseTranslation); - final double rotationDifferenceDegrees = Math.abs(pose.getRotation().minus(translationDifference.getAngle()).getDegrees()); - - return translationDistance * rotationToTranslation + rotationDifferenceDegrees * (1 - rotationToTranslation); - } - - private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); final Set objectsProcessed = new HashSet<>(); From a3f401feb4851b2f2e64c40a142769e0c9e0face Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 6 Jan 2026 18:42:37 +0200 Subject: [PATCH 24/68] removed distance calcultation method --- .../java/frc/trigon/robot/RobotContainer.java | 1 - .../objectDetection/ObjectPoseEstimator.java | 23 +------------------ 2 files changed, 1 insertion(+), 23 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 839a3ae3..ee86ec47 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -27,7 +27,6 @@ public class RobotContainer { 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 ); diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index be5702c6..436bd7bc 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -20,24 +20,21 @@ public class ObjectPoseEstimator extends SubsystemBase { private final SimulatedGamePieceConstants.GamePieceType gamePieceType; private final ObjectDetectionCamera[] cameras; private final HashMap knownObjectPositions; - private final double rotationToTranslation; /** * 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 distanceCalculationMethod the method used to calculate the distance from the game piece * @param gamePieceType the type of game piece to track * @param cameras the cameras used for detecting objects */ - public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, + public ObjectPoseEstimator(double deletionThresholdSeconds, SimulatedGamePieceConstants.GamePieceType gamePieceType, ObjectDetectionCamera... cameras) { this.deletionThresholdSeconds = deletionThresholdSeconds; this.gamePieceType = gamePieceType; this.cameras = cameras; this.knownObjectPositions = new HashMap<>(); - this.rotationToTranslation = distanceCalculationMethod.rotationToTranslation; } /** @@ -188,22 +185,4 @@ private void removeOldObjects() { private boolean isTooOld(double timestamp) { return Timer.getTimestamp() - timestamp > deletionThresholdSeconds; } - - public enum DistanceCalculationMethod { - ROTATION(0), - TRANSLATION(1), - ROTATION_AND_TRANSLATION(0.1); - - /** - * The ratio of rotation to translation in the distance rating calculation. - * A value of 0 means only rotation is considered, 1 means only translation is considered. - * Values in between are the ratio of rotation to translation in the distance rating calculation. - * For example, a value of 0.1 means that 9 cm of translation is considered equivalent to 1 degree of rotation. - */ - final double rotationToTranslation; - - DistanceCalculationMethod(double rotationToTranslation) { - this.rotationToTranslation = rotationToTranslation; - } - } } \ No newline at end of file From ea74422afbcaeaa7d1cb2376211a4f46aa1ef858 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 6 Jan 2026 18:55:48 +0200 Subject: [PATCH 25/68] more ranaming --- .../objectDetection/ObjectPoseEstimator.java | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 436bd7bc..80b15f70 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -19,7 +19,7 @@ public class ObjectPoseEstimator extends SubsystemBase { private final double deletionThresholdSeconds; private final SimulatedGamePieceConstants.GamePieceType gamePieceType; private final ObjectDetectionCamera[] cameras; - private final HashMap knownObjectPositions; + private final HashMap objectPositionsToTimeStamp; /** * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. @@ -34,7 +34,7 @@ public ObjectPoseEstimator(double deletionThresholdSeconds, this.deletionThresholdSeconds = deletionThresholdSeconds; this.gamePieceType = gamePieceType; this.cameras = cameras; - this.knownObjectPositions = new HashMap<>(); + this.objectPositionsToTimeStamp = new HashMap<>(); } /** @@ -54,7 +54,7 @@ public void periodic() { * @return a list of Translation2d representing the positions of objects on the field */ public ArrayList getObjectsOnField() { - return new ArrayList<>(knownObjectPositions.keySet()); + return new ArrayList<>(objectPositionsToTimeStamp.keySet()); } /** @@ -96,7 +96,7 @@ public void removeClosestObjectToPosition(Translation2d 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) { - knownObjectPositions.remove(objectPosition); + objectPositionsToTimeStamp.remove(objectPosition); } /** @@ -119,8 +119,8 @@ public Translation2d getClosestObjectToRobot() { * @return the closest object's position on the field, or null if no objects are known */ public Translation2d getClosestObjectToPosition(Translation2d position) { - final Translation2d[] objectsTranslations = knownObjectPositions.keySet().toArray(Translation2d[]::new); - if (knownObjectPositions.isEmpty()) + final Translation2d[] objectsTranslations = objectPositionsToTimeStamp.keySet().toArray(Translation2d[]::new); + if (objectPositionsToTimeStamp.isEmpty()) return null; Translation2d closestObjectTranslation = objectsTranslations[0]; double closestObjectDistance = position.getDistance(closestObjectTranslation); @@ -138,11 +138,11 @@ public Translation2d getClosestObjectToPosition(Translation2d position) { private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); - final Set objectsProcessed = new HashSet<>(); + final Set processedObjects = new HashSet<>(); for (ObjectDetectionCamera currentCamera : cameras) { for (Translation2d visibleObject : currentCamera.getObjectPositionsOnField(gamePieceType)) { - Translation2d closestKnownObjectToVisibleObject = getClosestKnownObjectToVisibleObject(visibleObject, objectsProcessed); + Translation2d closestKnownObjectToVisibleObject = getClosestKnownObjectToVisibleObject(visibleObject, processedObjects); double closestKnownObjectToVisibleObjectDistanceMeters = closestKnownObjectToVisibleObject != null ? visibleObject.getDistance(closestKnownObjectToVisibleObject) : Double.POSITIVE_INFINITY; @@ -150,23 +150,23 @@ private void updateObjectPositions() { if (shouldReplaceObject(closestKnownObjectToVisibleObject, closestKnownObjectToVisibleObjectDistanceMeters, currentTimestamp)) removeObject(closestKnownObjectToVisibleObject); - knownObjectPositions.put(visibleObject, currentTimestamp); - objectsProcessed.add(visibleObject); + objectPositionsToTimeStamp.put(visibleObject, currentTimestamp); + processedObjects.add(visibleObject); } } } - private boolean shouldReplaceObject(Translation2d object, double closestObjectToVisibleDistanceMeters, double currentTimestamp) { + private boolean shouldReplaceObject(Translation2d object, double closestObjectToVisibleObjectDistanceMeters, double currentTimestamp) { return object != null - && closestObjectToVisibleDistanceMeters < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS - && !knownObjectPositions.get(object).equals(currentTimestamp); + && closestObjectToVisibleObjectDistanceMeters < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS + && !objectPositionsToTimeStamp.get(object).equals(currentTimestamp); } private Translation2d getClosestKnownObjectToVisibleObject(Translation2d visibleObject, Set processedObjects) { Translation2d closestObjectToVisibleObject = null; double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; - for (Translation2d currentObject : knownObjectPositions.keySet()) { + for (Translation2d currentObject : objectPositionsToTimeStamp.keySet()) { if (!processedObjects.contains(currentObject)) { final double currentObjectDistanceMeters = visibleObject.getDistance(currentObject); if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { @@ -179,7 +179,7 @@ private Translation2d getClosestKnownObjectToVisibleObject(Translation2d visible } private void removeOldObjects() { - knownObjectPositions.entrySet().removeIf(entry -> isTooOld(entry.getValue())); + objectPositionsToTimeStamp.entrySet().removeIf(entry -> isTooOld(entry.getValue())); } private boolean isTooOld(double timestamp) { From bf047fad13ed23ae5519710ea453b820cb98063b Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 9 Jan 2026 00:46:18 +0200 Subject: [PATCH 26/68] changed updateObjectPositions to compare both cameras objects to the known objects --- .../objectDetection/ObjectPoseEstimator.java | 67 +++++++++++-------- .../ObjectDetectionCamera.java | 4 +- 2 files changed, 40 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 80b15f70..f85054d7 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -24,9 +24,9 @@ public class ObjectPoseEstimator extends SubsystemBase { /** * 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 cameras the cameras used for detecting objects + * @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 cameras the cameras used for detecting objects */ public ObjectPoseEstimator(double deletionThresholdSeconds, SimulatedGamePieceConstants.GamePieceType gamePieceType, @@ -119,8 +119,12 @@ public Translation2d getClosestObjectToRobot() { * @return the closest object's position on the field, or null if no objects are known */ public Translation2d getClosestObjectToPosition(Translation2d position) { - final Translation2d[] objectsTranslations = objectPositionsToTimeStamp.keySet().toArray(Translation2d[]::new); - if (objectPositionsToTimeStamp.isEmpty()) + return getClosestObjectFromSetToPosition(position, objectPositionsToTimeStamp.keySet()); + } + + public Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set recognizedObjects) { + final Translation2d[] objectsTranslations = recognizedObjects.toArray(Translation2d[]::new); + if (recognizedObjects.isEmpty()) return null; Translation2d closestObjectTranslation = objectsTranslations[0]; double closestObjectDistance = position.getDistance(closestObjectTranslation); @@ -138,41 +142,46 @@ public Translation2d getClosestObjectToPosition(Translation2d position) { private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); - final Set processedObjects = new HashSet<>(); - - for (ObjectDetectionCamera currentCamera : cameras) { - for (Translation2d visibleObject : currentCamera.getObjectPositionsOnField(gamePieceType)) { - Translation2d closestKnownObjectToVisibleObject = getClosestKnownObjectToVisibleObject(visibleObject, processedObjects); - double closestKnownObjectToVisibleObjectDistanceMeters = closestKnownObjectToVisibleObject != null - ? visibleObject.getDistance(closestKnownObjectToVisibleObject) - : Double.POSITIVE_INFINITY; + final Set recognizedObjects = new HashSet<>(); - if (shouldReplaceObject(closestKnownObjectToVisibleObject, closestKnownObjectToVisibleObjectDistanceMeters, currentTimestamp)) - removeObject(closestKnownObjectToVisibleObject); - - objectPositionsToTimeStamp.put(visibleObject, currentTimestamp); - processedObjects.add(visibleObject); + for (Translation2d currentObject : objectPositionsToTimeStamp.keySet()) { + recognizedObjects.clear(); + for (ObjectDetectionCamera currentCamera : cameras) { + for (Translation2d visibleObject : currentCamera.getObjectsPositionsOnField(gamePieceType)) { + if (isObjectWithinTolerance(visibleObject, currentObject)) + recognizedObjects.add(visibleObject); + else if (isObjectNew(visibleObject)) { + objectPositionsToTimeStamp.put(visibleObject, currentTimestamp); + } + } } + if (!recognizedObjects.isEmpty()) + updateObject(currentObject, getClosestObjectFromSetToPosition(currentObject, recognizedObjects), currentTimestamp); } } - private boolean shouldReplaceObject(Translation2d object, double closestObjectToVisibleObjectDistanceMeters, double currentTimestamp) { - return object != null - && closestObjectToVisibleObjectDistanceMeters < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS - && !objectPositionsToTimeStamp.get(object).equals(currentTimestamp); + private void updateObject(Translation2d oldObject, Translation2d newObject, double currentTimestamp) { + removeObject(oldObject); + objectPositionsToTimeStamp.put(newObject, currentTimestamp); } - private Translation2d getClosestKnownObjectToVisibleObject(Translation2d visibleObject, Set processedObjects) { + private boolean isObjectNew(Translation2d object) { + return !isObjectWithinTolerance(getClosestKnownObjectToVisibleObject(object), object); + } + + private boolean isObjectWithinTolerance(Translation2d firstObject, Translation2d secondObject) { + return firstObject.getDistance(secondObject) < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; + } + + private Translation2d getClosestKnownObjectToVisibleObject(Translation2d visibleObject) { Translation2d closestObjectToVisibleObject = null; double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; for (Translation2d currentObject : objectPositionsToTimeStamp.keySet()) { - if (!processedObjects.contains(currentObject)) { - final double currentObjectDistanceMeters = visibleObject.getDistance(currentObject); - if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { - closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; - closestObjectToVisibleObject = currentObject; - } + final double currentObjectDistanceMeters = visibleObject.getDistance(currentObject); + if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { + closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; + closestObjectToVisibleObject = currentObject; } } return closestObjectToVisibleObject; diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java index 2a8de8e5..cf97ae0f 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java @@ -39,7 +39,7 @@ public void periodic() { * @return the closest object's 2D position on the field (z is assumed to be 0) */ public Translation2d calculateClosestObjectPositionOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { - final Translation2d[] targetObjectsTranslation = getObjectPositionsOnField(targetGamePiece); + final Translation2d[] targetObjectsTranslation = getObjectsPositionsOnField(targetGamePiece); final Translation2d currentRobotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); if (targetObjectsTranslation.length == 0) return null; @@ -59,7 +59,7 @@ public boolean hasObject(SimulatedGamePieceConstants.GamePieceType targetGamePie return objectDetectionCameraInputs.hasObject[targetGamePiece.id]; } - public Translation2d[] getObjectPositionsOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + public Translation2d[] getObjectsPositionsOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { final Rotation3d[] visibleObjectsRotations = getObjectsRotations(targetGamePiece); final Translation2d[] objectsPositionsOnField = new Translation2d[visibleObjectsRotations.length]; From 4ddf1a49de5d0ba14add38fcec8a9b69413d86b5 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 9 Jan 2026 00:59:39 +0200 Subject: [PATCH 27/68] fixed issue --- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index f85054d7..84a4c98e 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -143,6 +143,7 @@ public Translation2d getClosestObjectFromSetToPosition(Translation2d position, S private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); final Set recognizedObjects = new HashSet<>(); + final HashMap newObjects = new HashMap<>(); for (Translation2d currentObject : objectPositionsToTimeStamp.keySet()) { recognizedObjects.clear(); @@ -151,13 +152,14 @@ private void updateObjectPositions() { if (isObjectWithinTolerance(visibleObject, currentObject)) recognizedObjects.add(visibleObject); else if (isObjectNew(visibleObject)) { - objectPositionsToTimeStamp.put(visibleObject, currentTimestamp); + newObjects.put(visibleObject, currentTimestamp); } } } if (!recognizedObjects.isEmpty()) updateObject(currentObject, getClosestObjectFromSetToPosition(currentObject, recognizedObjects), currentTimestamp); } + objectPositionsToTimeStamp.putAll(newObjects); } private void updateObject(Translation2d oldObject, Translation2d newObject, double currentTimestamp) { @@ -166,6 +168,8 @@ private void updateObject(Translation2d oldObject, Translation2d newObject, doub } private boolean isObjectNew(Translation2d object) { + if (objectPositionsToTimeStamp.isEmpty()) + return true; return !isObjectWithinTolerance(getClosestKnownObjectToVisibleObject(object), object); } From c90f8f36f92fcca24865c6dee450c3e32d2af448 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 9 Jan 2026 01:53:17 +0200 Subject: [PATCH 28/68] made object updating function more efficient and separated into more functions --- .../objectDetection/ObjectPoseEstimator.java | 49 ++++++++++--------- 1 file changed, 27 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 84a4c98e..9cb466fc 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -10,10 +10,7 @@ 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; +import java.util.*; public class ObjectPoseEstimator extends SubsystemBase { private final double deletionThresholdSeconds; @@ -122,9 +119,9 @@ public Translation2d getClosestObjectToPosition(Translation2d position) { return getClosestObjectFromSetToPosition(position, objectPositionsToTimeStamp.keySet()); } - public Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set recognizedObjects) { - final Translation2d[] objectsTranslations = recognizedObjects.toArray(Translation2d[]::new); - if (recognizedObjects.isEmpty()) + public Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set objects) { + final Translation2d[] objectsTranslations = objects.toArray(Translation2d[]::new); + if (objects.isEmpty()) return null; Translation2d closestObjectTranslation = objectsTranslations[0]; double closestObjectDistance = position.getDistance(closestObjectTranslation); @@ -142,24 +139,32 @@ public Translation2d getClosestObjectFromSetToPosition(Translation2d position, S private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); - final Set recognizedObjects = new HashSet<>(); - final HashMap newObjects = new HashMap<>(); + final Set visibleObjects = getVisibleObjects(); + updateKnownObjects(visibleObjects, currentTimestamp); + addNewObjects(visibleObjects, currentTimestamp); + } + + private void updateKnownObjects(Set newObjects, double currentTimestamp) { for (Translation2d currentObject : objectPositionsToTimeStamp.keySet()) { - recognizedObjects.clear(); - for (ObjectDetectionCamera currentCamera : cameras) { - for (Translation2d visibleObject : currentCamera.getObjectsPositionsOnField(gamePieceType)) { - if (isObjectWithinTolerance(visibleObject, currentObject)) - recognizedObjects.add(visibleObject); - else if (isObjectNew(visibleObject)) { - newObjects.put(visibleObject, currentTimestamp); - } - } - } - if (!recognizedObjects.isEmpty()) - updateObject(currentObject, getClosestObjectFromSetToPosition(currentObject, recognizedObjects), currentTimestamp); + Translation2d closestNewObjectToCurrentObject = getClosestObjectFromSetToPosition(currentObject, newObjects); + if (isObjectWithinTolerance(closestNewObjectToCurrentObject, currentObject)) + updateObject(currentObject, closestNewObjectToCurrentObject, currentTimestamp); + } + } + + private void addNewObjects(Set objects, double currentTimestamp) { + for (Translation2d object : objects) { + if (isObjectNew(object)) + objectPositionsToTimeStamp.put(object, currentTimestamp); } - objectPositionsToTimeStamp.putAll(newObjects); + } + + private Set getVisibleObjects() { + final Set visibleObjects = new HashSet<>(); + for (ObjectDetectionCamera currentCamera : cameras) + visibleObjects.addAll(Arrays.asList(currentCamera.getObjectsPositionsOnField(gamePieceType))); + return visibleObjects; } private void updateObject(Translation2d oldObject, Translation2d newObject, double currentTimestamp) { From dd4e9bddf44b9ffdfe33ca22189f3404c0b7e830 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sat, 10 Jan 2026 18:15:04 +0200 Subject: [PATCH 29/68] updated the function --- .../objectDetection/ObjectPoseEstimator.java | 55 ++++++++++--------- 1 file changed, 28 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 9cb466fc..784815b6 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -139,37 +139,38 @@ public Translation2d getClosestObjectFromSetToPosition(Translation2d position, S private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); - final Set visibleObjects = getVisibleObjects(); - - updateKnownObjects(visibleObjects, currentTimestamp); - addNewObjects(visibleObjects, currentTimestamp); - } - - private void updateKnownObjects(Set newObjects, double currentTimestamp) { - for (Translation2d currentObject : objectPositionsToTimeStamp.keySet()) { - Translation2d closestNewObjectToCurrentObject = getClosestObjectFromSetToPosition(currentObject, newObjects); - if (isObjectWithinTolerance(closestNewObjectToCurrentObject, currentObject)) - updateObject(currentObject, closestNewObjectToCurrentObject, currentTimestamp); + HashMap objectsToUpdate = new HashMap<>(); + + for (ObjectDetectionCamera camera : cameras) { + for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { + final Translation2d closestObjectToVisibleObject = getClosestKnownObjectToVisibleObject(visibleObject); + if (isObjectNew(visibleObject)) { + objectPositionsToTimeStamp.put(visibleObject, currentTimestamp); + objectsToUpdate.put(visibleObject, visibleObject); + } + + if (visibleObject.getDistance(closestObjectToVisibleObject) < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) { + objectsToUpdate = updateObject(visibleObject, objectsToUpdate); + } + } } - } - - private void addNewObjects(Set objects, double currentTimestamp) { - for (Translation2d object : objects) { - if (isObjectNew(object)) - objectPositionsToTimeStamp.put(object, currentTimestamp); + for (Translation2d object : objectsToUpdate.keySet()) { + objectPositionsToTimeStamp.remove(object); + objectPositionsToTimeStamp.put(object, currentTimestamp); } } - private Set getVisibleObjects() { - final Set visibleObjects = new HashSet<>(); - for (ObjectDetectionCamera currentCamera : cameras) - visibleObjects.addAll(Arrays.asList(currentCamera.getObjectsPositionsOnField(gamePieceType))); - return visibleObjects; - } - - private void updateObject(Translation2d oldObject, Translation2d newObject, double currentTimestamp) { - removeObject(oldObject); - objectPositionsToTimeStamp.put(newObject, currentTimestamp); + private HashMap updateObject(Translation2d object, HashMap objectsToUpdate) { + Translation2d closestKnownObject = getClosestKnownObjectToVisibleObject(object); + double distanceBetweenObjects = object.getDistance(closestKnownObject); + if (objectsToUpdate.containsKey(closestKnownObject)) { + final Translation2d closestKnownObjectUpdate = objectsToUpdate.get(closestKnownObject); + if (distanceBetweenObjects < closestKnownObjectUpdate.getDistance(closestKnownObject)) { + objectsToUpdate.replace(closestKnownObject, object); + updateObject(closestKnownObjectUpdate, objectsToUpdate); + } + } + return objectsToUpdate; } private boolean isObjectNew(Translation2d object) { From 3fe913ec1c3081aa87b651beaeb88465b7071fb0 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 13:59:56 +0200 Subject: [PATCH 30/68] updated function again --- .../objectDetection/ObjectPoseEstimator.java | 40 +++++++++---------- 1 file changed, 18 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 784815b6..02e69d1b 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -10,7 +10,9 @@ import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import org.littletonrobotics.junction.Logger; -import java.util.*; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.Set; public class ObjectPoseEstimator extends SubsystemBase { private final double deletionThresholdSeconds; @@ -143,34 +145,28 @@ private void updateObjectPositions() { for (ObjectDetectionCamera camera : cameras) { for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { - final Translation2d closestObjectToVisibleObject = getClosestKnownObjectToVisibleObject(visibleObject); if (isObjectNew(visibleObject)) { - objectPositionsToTimeStamp.put(visibleObject, currentTimestamp); objectsToUpdate.put(visibleObject, visibleObject); + continue; } - if (visibleObject.getDistance(closestObjectToVisibleObject) < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) { - objectsToUpdate = updateObject(visibleObject, objectsToUpdate); + final Translation2d closestObjectToVisibleObject = getClosestKnownObjectToVisibleObject(visibleObject); + final double closestObjectToVisibleObjectDistanceMeters = visibleObject.getDistance(closestObjectToVisibleObject); + + if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) { + objectsToUpdate.merge( + closestObjectToVisibleObject, + visibleObject, + (oldVisibleObject, currentVisibleObject) -> + currentVisibleObject.getDistance(closestObjectToVisibleObject) < oldVisibleObject.getDistance(closestObjectToVisibleObject) + ? currentVisibleObject + : oldVisibleObject + ); } } } - for (Translation2d object : objectsToUpdate.keySet()) { - objectPositionsToTimeStamp.remove(object); - objectPositionsToTimeStamp.put(object, currentTimestamp); - } - } - - private HashMap updateObject(Translation2d object, HashMap objectsToUpdate) { - Translation2d closestKnownObject = getClosestKnownObjectToVisibleObject(object); - double distanceBetweenObjects = object.getDistance(closestKnownObject); - if (objectsToUpdate.containsKey(closestKnownObject)) { - final Translation2d closestKnownObjectUpdate = objectsToUpdate.get(closestKnownObject); - if (distanceBetweenObjects < closestKnownObjectUpdate.getDistance(closestKnownObject)) { - objectsToUpdate.replace(closestKnownObject, object); - updateObject(closestKnownObjectUpdate, objectsToUpdate); - } - } - return objectsToUpdate; + objectsToUpdate.keySet().forEach(objectPositionsToTimeStamp::remove); + objectsToUpdate.values().forEach(object -> objectPositionsToTimeStamp.put(object, currentTimestamp)); } private boolean isObjectNew(Translation2d object) { From e81df9ff22208bfacdd4e9b3ec57dffeb4b21a86 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 14:06:52 +0200 Subject: [PATCH 31/68] removed redundant function --- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 02e69d1b..69029f99 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -172,11 +172,7 @@ private void updateObjectPositions() { private boolean isObjectNew(Translation2d object) { if (objectPositionsToTimeStamp.isEmpty()) return true; - return !isObjectWithinTolerance(getClosestKnownObjectToVisibleObject(object), object); - } - - private boolean isObjectWithinTolerance(Translation2d firstObject, Translation2d secondObject) { - return firstObject.getDistance(secondObject) < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; + return object.getDistance(getClosestKnownObjectToVisibleObject(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } private Translation2d getClosestKnownObjectToVisibleObject(Translation2d visibleObject) { From 687bef3052e5f3adcbf5ceb9fe9a27a00090ca64 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 14:23:16 +0200 Subject: [PATCH 32/68] extracted into another function for readability --- .../objectDetection/ObjectPoseEstimator.java | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 69029f99..a2916d2c 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -13,6 +13,7 @@ import java.util.ArrayList; import java.util.HashMap; import java.util.Set; +import java.util.function.BiFunction; public class ObjectPoseEstimator extends SubsystemBase { private final double deletionThresholdSeconds; @@ -122,9 +123,9 @@ public Translation2d getClosestObjectToPosition(Translation2d position) { } public Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set objects) { - final Translation2d[] objectsTranslations = objects.toArray(Translation2d[]::new); if (objects.isEmpty()) return null; + final Translation2d[] objectsTranslations = objects.toArray(Translation2d[]::new); Translation2d closestObjectTranslation = objectsTranslations[0]; double closestObjectDistance = position.getDistance(closestObjectTranslation); @@ -153,22 +154,21 @@ private void updateObjectPositions() { final Translation2d closestObjectToVisibleObject = getClosestKnownObjectToVisibleObject(visibleObject); final double closestObjectToVisibleObjectDistanceMeters = visibleObject.getDistance(closestObjectToVisibleObject); - if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) { - objectsToUpdate.merge( - closestObjectToVisibleObject, - visibleObject, - (oldVisibleObject, currentVisibleObject) -> - currentVisibleObject.getDistance(closestObjectToVisibleObject) < oldVisibleObject.getDistance(closestObjectToVisibleObject) - ? currentVisibleObject - : oldVisibleObject - ); - } + if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) + objectsToUpdate.merge(closestObjectToVisibleObject, visibleObject, getClosestVisibleObjectToPosition(closestObjectToVisibleObject)); } } objectsToUpdate.keySet().forEach(objectPositionsToTimeStamp::remove); objectsToUpdate.values().forEach(object -> objectPositionsToTimeStamp.put(object, currentTimestamp)); } + private BiFunction getClosestVisibleObjectToPosition(Translation2d position) { + return (oldVisibleObject, currentVisibleObject) -> + currentVisibleObject.getDistance(position) < oldVisibleObject.getDistance(position) + ? currentVisibleObject + : oldVisibleObject; + } + private boolean isObjectNew(Translation2d object) { if (objectPositionsToTimeStamp.isEmpty()) return true; From 5f142dda00cbe437a8b3d333b2a06ee8d51adac5 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 14:25:20 +0200 Subject: [PATCH 33/68] rename --- .../misc/objectDetection/ObjectPoseEstimator.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index a2916d2c..f16db0bf 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -142,12 +142,12 @@ public Translation2d getClosestObjectFromSetToPosition(Translation2d position, S private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); - HashMap objectsToUpdate = new HashMap<>(); + HashMap currentToNewObjectPositions = new HashMap<>(); for (ObjectDetectionCamera camera : cameras) { for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { if (isObjectNew(visibleObject)) { - objectsToUpdate.put(visibleObject, visibleObject); + currentToNewObjectPositions.put(visibleObject, visibleObject); continue; } @@ -155,11 +155,11 @@ private void updateObjectPositions() { final double closestObjectToVisibleObjectDistanceMeters = visibleObject.getDistance(closestObjectToVisibleObject); if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) - objectsToUpdate.merge(closestObjectToVisibleObject, visibleObject, getClosestVisibleObjectToPosition(closestObjectToVisibleObject)); + currentToNewObjectPositions.merge(closestObjectToVisibleObject, visibleObject, getClosestVisibleObjectToPosition(closestObjectToVisibleObject)); } } - objectsToUpdate.keySet().forEach(objectPositionsToTimeStamp::remove); - objectsToUpdate.values().forEach(object -> objectPositionsToTimeStamp.put(object, currentTimestamp)); + currentToNewObjectPositions.keySet().forEach(objectPositionsToTimeStamp::remove); + currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimeStamp.put(object, currentTimestamp)); } private BiFunction getClosestVisibleObjectToPosition(Translation2d position) { From 471d800cddee083b9deaa8b96a12008fb33e06d1 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 14:43:55 +0200 Subject: [PATCH 34/68] fixed logic issue --- .../objectDetection/ObjectPoseEstimator.java | 30 ++++++------------- 1 file changed, 9 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index f16db0bf..1af34f50 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -146,15 +146,13 @@ private void updateObjectPositions() { for (ObjectDetectionCamera camera : cameras) { for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { - if (isObjectNew(visibleObject)) { + final Translation2d closestObjectToVisibleObject = getClosestObjectToPosition(visibleObject); + + if (isObjectNew(visibleObject) && !isObjectsDistanceWithinTolerance(visibleObject, getClosestObjectFromSetToPosition(visibleObject, currentToNewObjectPositions.keySet()))) { currentToNewObjectPositions.put(visibleObject, visibleObject); continue; } - - final Translation2d closestObjectToVisibleObject = getClosestKnownObjectToVisibleObject(visibleObject); - final double closestObjectToVisibleObjectDistanceMeters = visibleObject.getDistance(closestObjectToVisibleObject); - - if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) + if (isObjectsDistanceWithinTolerance(visibleObject, closestObjectToVisibleObject)) currentToNewObjectPositions.merge(closestObjectToVisibleObject, visibleObject, getClosestVisibleObjectToPosition(closestObjectToVisibleObject)); } } @@ -169,24 +167,14 @@ private void updateObjectPositions() { : oldVisibleObject; } + private boolean isObjectsDistanceWithinTolerance(Translation2d firstObject, Translation2d secondObject) { + return firstObject.getDistance(secondObject) < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; + } + private boolean isObjectNew(Translation2d object) { if (objectPositionsToTimeStamp.isEmpty()) return true; - return object.getDistance(getClosestKnownObjectToVisibleObject(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; - } - - private Translation2d getClosestKnownObjectToVisibleObject(Translation2d visibleObject) { - Translation2d closestObjectToVisibleObject = null; - double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; - - for (Translation2d currentObject : objectPositionsToTimeStamp.keySet()) { - final double currentObjectDistanceMeters = visibleObject.getDistance(currentObject); - if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { - closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; - closestObjectToVisibleObject = currentObject; - } - } - return closestObjectToVisibleObject; + return object.getDistance(getClosestObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } private void removeOldObjects() { From e60a7ba2189752884373c42392d0675e8fa3dd27 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 14:47:00 +0200 Subject: [PATCH 35/68] rename --- .../objectDetection/ObjectPoseEstimator.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 1af34f50..b61a6979 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -72,7 +72,7 @@ public void removeClosestObjectToRobot() { */ public void removeClosestObjectToIntake(Transform2d intakeTransform) { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - removeObject(getClosestObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation())); + removeObject(getClosestKnownObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation())); } /** @@ -81,12 +81,12 @@ public void removeClosestObjectToIntake(Transform2d intakeTransform) { * @param fieldRelativePose the pose to which the removed object is closest */ public void removeClosestObjectToPose(Pose2d fieldRelativePose) { - final Translation2d closestObject = getClosestObjectToPosition(fieldRelativePose.getTranslation()); + final Translation2d closestObject = getClosestKnownObjectToPosition(fieldRelativePose.getTranslation()); removeObject(closestObject); } public void removeClosestObjectToPosition(Translation2d position) { - final Translation2d closestObject = getClosestObjectToPosition(position); + final Translation2d closestObject = getClosestKnownObjectToPosition(position); removeObject(closestObject); } @@ -108,7 +108,7 @@ public void removeObject(Translation2d objectPosition) { * @return the best object's 2D position on the field (z is assumed to be 0) */ public Translation2d getClosestObjectToRobot() { - return getClosestObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); + return getClosestKnownObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); } @@ -118,7 +118,7 @@ public Translation2d getClosestObjectToRobot() { * @param position the position to which the returned object is closest * @return the closest object's position on the field, or null if no objects are known */ - public Translation2d getClosestObjectToPosition(Translation2d position) { + public Translation2d getClosestKnownObjectToPosition(Translation2d position) { return getClosestObjectFromSetToPosition(position, objectPositionsToTimeStamp.keySet()); } @@ -146,21 +146,21 @@ private void updateObjectPositions() { for (ObjectDetectionCamera camera : cameras) { for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { - final Translation2d closestObjectToVisibleObject = getClosestObjectToPosition(visibleObject); + final Translation2d closestObjectToVisibleObject = getClosestKnownObjectToPosition(visibleObject); if (isObjectNew(visibleObject) && !isObjectsDistanceWithinTolerance(visibleObject, getClosestObjectFromSetToPosition(visibleObject, currentToNewObjectPositions.keySet()))) { currentToNewObjectPositions.put(visibleObject, visibleObject); continue; } if (isObjectsDistanceWithinTolerance(visibleObject, closestObjectToVisibleObject)) - currentToNewObjectPositions.merge(closestObjectToVisibleObject, visibleObject, getClosestVisibleObjectToPosition(closestObjectToVisibleObject)); + currentToNewObjectPositions.merge(closestObjectToVisibleObject, visibleObject, getClosestVisibleObjectFunction(closestObjectToVisibleObject)); } } currentToNewObjectPositions.keySet().forEach(objectPositionsToTimeStamp::remove); currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimeStamp.put(object, currentTimestamp)); } - private BiFunction getClosestVisibleObjectToPosition(Translation2d position) { + private BiFunction getClosestVisibleObjectFunction(Translation2d position) { return (oldVisibleObject, currentVisibleObject) -> currentVisibleObject.getDistance(position) < oldVisibleObject.getDistance(position) ? currentVisibleObject @@ -174,7 +174,7 @@ private boolean isObjectsDistanceWithinTolerance(Translation2d firstObject, Tran private boolean isObjectNew(Translation2d object) { if (objectPositionsToTimeStamp.isEmpty()) return true; - return object.getDistance(getClosestObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; + return object.getDistance(getClosestKnownObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } private void removeOldObjects() { From a01fda930470fdcd0cfd1cb4f2488224a824d19a Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 15:26:06 +0200 Subject: [PATCH 36/68] added javadoc --- .../objectDetection/ObjectPoseEstimator.java | 27 +++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index b61a6979..4a693ad8 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -122,7 +122,7 @@ public Translation2d getClosestKnownObjectToPosition(Translation2d position) { return getClosestObjectFromSetToPosition(position, objectPositionsToTimeStamp.keySet()); } - public Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set objects) { + private Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set objects) { if (objects.isEmpty()) return null; final Translation2d[] objectsTranslations = objects.toArray(Translation2d[]::new); @@ -140,6 +140,23 @@ public Translation2d getClosestObjectFromSetToPosition(Translation2d position, S return closestObjectTranslation; } + /** + * Updates the positions of previously detected objects, or adds them if they are new. + * + *

{@code currentToNewObjectPositions} maps previously detected objects to the + * positions they should be updated to.

+ * + *

Each object detected by the camera(s) is compared to the closest previously + * detected object to determine whether it is new or already tracked. If it is + * already tracked, it is added to {@code currentToNewObjectPositions} as an update.

+ * + *

If multiple objects detected by the camera(s) are closest to the same + * previously detected object, the object whose position is closest to that + * previously detected object is kept. The others are discarded.

+ * + *

This assumes that such detections represent the same physical object seen + * by different cameras, with small positional differences.

+ */ private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); HashMap currentToNewObjectPositions = new HashMap<>(); @@ -153,7 +170,13 @@ private void updateObjectPositions() { continue; } if (isObjectsDistanceWithinTolerance(visibleObject, closestObjectToVisibleObject)) - currentToNewObjectPositions.merge(closestObjectToVisibleObject, visibleObject, getClosestVisibleObjectFunction(closestObjectToVisibleObject)); + currentToNewObjectPositions.merge( + closestObjectToVisibleObject, + visibleObject, + (oldVisibleObject, currentVisibleObject) -> + currentVisibleObject.getDistance(closestObjectToVisibleObject) < oldVisibleObject.getDistance(closestObjectToVisibleObject) + ? currentVisibleObject + : oldVisibleObject); } } currentToNewObjectPositions.keySet().forEach(objectPositionsToTimeStamp::remove); From dd177865fa910e8014d94bc4566499bce0b659aa Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 15:36:09 +0200 Subject: [PATCH 37/68] fix --- .../commands/commandclasses/IntakeAssistCommand.java | 2 +- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 9 +++++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index e8595304..fce03ab1 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/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; } diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 4a693ad8..ee8d8661 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -99,6 +99,15 @@ public void removeObject(Translation2d objectPosition) { objectPositionsToTimeStamp.remove(objectPosition); } + /** + * determines whether any objects are stored in the poseEstimator. + * + * @return if there are objects stored in the poseEstimator + */ + public boolean hasObjects() { + return !getObjectsOnField().isEmpty(); + } + /** * Gets the position of the closest object on the field from the 3D rotation of the object relative to the camera. * This assumes the object is on the ground. From d607c896c2da211962aaf6cb45b83c8a0388cffe Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 15:41:10 +0200 Subject: [PATCH 38/68] fixed issue --- .../misc/objectDetection/ObjectPoseEstimator.java | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index ee8d8661..f9104dc3 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -13,7 +13,6 @@ import java.util.ArrayList; import java.util.HashMap; import java.util.Set; -import java.util.function.BiFunction; public class ObjectPoseEstimator extends SubsystemBase { private final double deletionThresholdSeconds; @@ -192,14 +191,9 @@ private void updateObjectPositions() { currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimeStamp.put(object, currentTimestamp)); } - private BiFunction getClosestVisibleObjectFunction(Translation2d position) { - return (oldVisibleObject, currentVisibleObject) -> - currentVisibleObject.getDistance(position) < oldVisibleObject.getDistance(position) - ? currentVisibleObject - : oldVisibleObject; - } - private boolean isObjectsDistanceWithinTolerance(Translation2d firstObject, Translation2d secondObject) { + if (firstObject == null || secondObject == null) + return false; return firstObject.getDistance(secondObject) < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } From d49a98a4d8796d1de1f2e1a29514edbf60c72eb6 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 21:36:39 +0200 Subject: [PATCH 39/68] extracted into separate functions --- .../objectDetection/ObjectPoseEstimator.java | 27 +++++++++++-------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index f9104dc3..d5294ede 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -173,34 +173,39 @@ private void updateObjectPositions() { for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { final Translation2d closestObjectToVisibleObject = getClosestKnownObjectToPosition(visibleObject); - if (isObjectNew(visibleObject) && !isObjectsDistanceWithinTolerance(visibleObject, getClosestObjectFromSetToPosition(visibleObject, currentToNewObjectPositions.keySet()))) { + if (isObjectNew(visibleObject, currentToNewObjectPositions)) { currentToNewObjectPositions.put(visibleObject, visibleObject); continue; } if (isObjectsDistanceWithinTolerance(visibleObject, closestObjectToVisibleObject)) - currentToNewObjectPositions.merge( - closestObjectToVisibleObject, - visibleObject, - (oldVisibleObject, currentVisibleObject) -> - currentVisibleObject.getDistance(closestObjectToVisibleObject) < oldVisibleObject.getDistance(closestObjectToVisibleObject) - ? currentVisibleObject - : oldVisibleObject); + currentToNewObjectPositions = updateHashMapObject(visibleObject, closestObjectToVisibleObject, currentToNewObjectPositions); } } currentToNewObjectPositions.keySet().forEach(objectPositionsToTimeStamp::remove); currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimeStamp.put(object, currentTimestamp)); } + private HashMap updateHashMapObject(Translation2d objectUpdate, Translation2d closestObjectToObjectUpdate, HashMap objectsToUpdate) { + if (objectsToUpdate.containsKey(closestObjectToObjectUpdate)) { + final Translation2d closestKnownObjectPreviousUpdate = objectsToUpdate.get(closestObjectToObjectUpdate); + if (objectUpdate.getDistance(closestObjectToObjectUpdate) < closestKnownObjectPreviousUpdate.getDistance(closestObjectToObjectUpdate)) { + objectsToUpdate.replace(closestObjectToObjectUpdate, objectUpdate); + } + } + return objectsToUpdate; + } + private boolean isObjectsDistanceWithinTolerance(Translation2d firstObject, Translation2d secondObject) { if (firstObject == null || secondObject == null) return false; return firstObject.getDistance(secondObject) < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } - private boolean isObjectNew(Translation2d object) { - if (objectPositionsToTimeStamp.isEmpty()) + private boolean isObjectNew(Translation2d object, HashMap currentToNewObjectPositions) { + if (objectPositionsToTimeStamp.isEmpty() && currentToNewObjectPositions.isEmpty()) return true; - return object.getDistance(getClosestKnownObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; + return object.getDistance(getClosestKnownObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS + && !isObjectsDistanceWithinTolerance(object, getClosestObjectFromSetToPosition(object, currentToNewObjectPositions.keySet())); } private void removeOldObjects() { From 9ddfda6a44bfbbed1bc5a3ddabb4334d250405c7 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 21:51:38 +0200 Subject: [PATCH 40/68] fixed issue --- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index d5294ede..8cbafb29 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -188,10 +188,10 @@ private void updateObjectPositions() { private HashMap updateHashMapObject(Translation2d objectUpdate, Translation2d closestObjectToObjectUpdate, HashMap objectsToUpdate) { if (objectsToUpdate.containsKey(closestObjectToObjectUpdate)) { final Translation2d closestKnownObjectPreviousUpdate = objectsToUpdate.get(closestObjectToObjectUpdate); - if (objectUpdate.getDistance(closestObjectToObjectUpdate) < closestKnownObjectPreviousUpdate.getDistance(closestObjectToObjectUpdate)) { + if (objectUpdate.getDistance(closestObjectToObjectUpdate) < closestKnownObjectPreviousUpdate.getDistance(closestObjectToObjectUpdate)) objectsToUpdate.replace(closestObjectToObjectUpdate, objectUpdate); - } - } + } else + objectsToUpdate.put(closestObjectToObjectUpdate, objectUpdate); return objectsToUpdate; } From b0e8f324e12d9fc3234e565d084d214a522a4445 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 22:57:19 +0200 Subject: [PATCH 41/68] reverted to one camera --- .../objectDetection/ObjectPoseEstimator.java | 24 +++++++++---------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 8cbafb29..d081057e 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -17,7 +17,7 @@ public class ObjectPoseEstimator extends SubsystemBase { private final double deletionThresholdSeconds; private final SimulatedGamePieceConstants.GamePieceType gamePieceType; - private final ObjectDetectionCamera[] cameras; + private final ObjectDetectionCamera camera; private final HashMap objectPositionsToTimeStamp; /** @@ -25,14 +25,14 @@ public class ObjectPoseEstimator extends SubsystemBase { * * @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 cameras the cameras used for detecting objects + * @param camera the camera used for detecting objects */ public ObjectPoseEstimator(double deletionThresholdSeconds, SimulatedGamePieceConstants.GamePieceType gamePieceType, - ObjectDetectionCamera... cameras) { + ObjectDetectionCamera camera) { this.deletionThresholdSeconds = deletionThresholdSeconds; this.gamePieceType = gamePieceType; - this.cameras = cameras; + this.camera = camera; this.objectPositionsToTimeStamp = new HashMap<>(); } @@ -169,17 +169,15 @@ private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); HashMap currentToNewObjectPositions = new HashMap<>(); - for (ObjectDetectionCamera camera : cameras) { - for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { - final Translation2d closestObjectToVisibleObject = getClosestKnownObjectToPosition(visibleObject); + for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { + final Translation2d closestObjectToVisibleObject = getClosestKnownObjectToPosition(visibleObject); - if (isObjectNew(visibleObject, currentToNewObjectPositions)) { - currentToNewObjectPositions.put(visibleObject, visibleObject); - continue; - } - if (isObjectsDistanceWithinTolerance(visibleObject, closestObjectToVisibleObject)) - currentToNewObjectPositions = updateHashMapObject(visibleObject, closestObjectToVisibleObject, currentToNewObjectPositions); + if (isObjectNew(visibleObject, currentToNewObjectPositions)) { + currentToNewObjectPositions.put(visibleObject, visibleObject); + continue; } + if (isObjectsDistanceWithinTolerance(visibleObject, closestObjectToVisibleObject)) + currentToNewObjectPositions = updateHashMapObject(visibleObject, closestObjectToVisibleObject, currentToNewObjectPositions); } currentToNewObjectPositions.keySet().forEach(objectPositionsToTimeStamp::remove); currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimeStamp.put(object, currentTimestamp)); From 124ea0a72e7e595be60f88485d2b251b530fd1d1 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 22:59:53 +0200 Subject: [PATCH 42/68] Update ObjectPoseEstimator.java --- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index d081057e..0fe08302 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -186,8 +186,10 @@ private void updateObjectPositions() { private HashMap updateHashMapObject(Translation2d objectUpdate, Translation2d closestObjectToObjectUpdate, HashMap objectsToUpdate) { if (objectsToUpdate.containsKey(closestObjectToObjectUpdate)) { final Translation2d closestKnownObjectPreviousUpdate = objectsToUpdate.get(closestObjectToObjectUpdate); - if (objectUpdate.getDistance(closestObjectToObjectUpdate) < closestKnownObjectPreviousUpdate.getDistance(closestObjectToObjectUpdate)) + if (objectUpdate.getDistance(closestObjectToObjectUpdate) < closestKnownObjectPreviousUpdate.getDistance(closestObjectToObjectUpdate)) { objectsToUpdate.replace(closestObjectToObjectUpdate, objectUpdate); + objectsToUpdate.put(closestKnownObjectPreviousUpdate, closestKnownObjectPreviousUpdate); + } } else objectsToUpdate.put(closestObjectToObjectUpdate, objectUpdate); return objectsToUpdate; From 172bef1cbc934ebbadff0f87a4b33bc193fd434c Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 11 Jan 2026 23:23:00 +0200 Subject: [PATCH 43/68] Update ObjectPoseEstimator.java --- .../objectDetection/ObjectPoseEstimator.java | 103 +++++++----------- 1 file changed, 40 insertions(+), 63 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 0fe08302..e0e298e7 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -119,70 +119,32 @@ public Translation2d getClosestObjectToRobot() { return getClosestKnownObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); } + private void updateObjectPositions() { + HashMap currentToNewObjectPositions = new HashMap<>(); - /** - * Gets the position of the closest object to a given position on the field. - * - * @param position the position to which the returned object is closest - * @return the closest object's position on the field, or null if no objects are known - */ - public Translation2d getClosestKnownObjectToPosition(Translation2d position) { - return getClosestObjectFromSetToPosition(position, objectPositionsToTimeStamp.keySet()); - } + for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) + currentToNewObjectPositions = updateObjectPosition(visibleObject, currentToNewObjectPositions); - private Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set objects) { - if (objects.isEmpty()) - return null; - final Translation2d[] objectsTranslations = objects.toArray(Translation2d[]::new); - Translation2d closestObjectTranslation = objectsTranslations[0]; - double closestObjectDistance = position.getDistance(closestObjectTranslation); - - for (int i = 1; i < objectsTranslations.length; i++) { - final Translation2d currentObjectTranslation = objectsTranslations[i]; - final double currentObjectDistance = position.getDistance(currentObjectTranslation); - if (currentObjectDistance < closestObjectDistance) { - closestObjectDistance = currentObjectDistance; - closestObjectTranslation = currentObjectTranslation; - } - } - return closestObjectTranslation; + mergeObjectsIntoHashmap(currentToNewObjectPositions); } - /** - * Updates the positions of previously detected objects, or adds them if they are new. - * - *

{@code currentToNewObjectPositions} maps previously detected objects to the - * positions they should be updated to.

- * - *

Each object detected by the camera(s) is compared to the closest previously - * detected object to determine whether it is new or already tracked. If it is - * already tracked, it is added to {@code currentToNewObjectPositions} as an update.

- * - *

If multiple objects detected by the camera(s) are closest to the same - * previously detected object, the object whose position is closest to that - * previously detected object is kept. The others are discarded.

- * - *

This assumes that such detections represent the same physical object seen - * by different cameras, with small positional differences.

- */ - private void updateObjectPositions() { + private void mergeObjectsIntoHashmap(HashMap currentToNewObjectPositions) { final double currentTimestamp = Timer.getTimestamp(); - HashMap currentToNewObjectPositions = new HashMap<>(); - for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { - final Translation2d closestObjectToVisibleObject = getClosestKnownObjectToPosition(visibleObject); - - if (isObjectNew(visibleObject, currentToNewObjectPositions)) { - currentToNewObjectPositions.put(visibleObject, visibleObject); - continue; - } - if (isObjectsDistanceWithinTolerance(visibleObject, closestObjectToVisibleObject)) - currentToNewObjectPositions = updateHashMapObject(visibleObject, closestObjectToVisibleObject, currentToNewObjectPositions); - } currentToNewObjectPositions.keySet().forEach(objectPositionsToTimeStamp::remove); currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimeStamp.put(object, currentTimestamp)); } + private HashMap updateObjectPosition(Translation2d object, HashMap currentToNewObjectPositions) { + final Translation2d closestObjectToTargetObject = getClosestKnownObjectToPosition(object); + + if (isObjectNew(object)) + currentToNewObjectPositions.put(object, object); + else + currentToNewObjectPositions = updateHashMapObject(object, closestObjectToTargetObject, currentToNewObjectPositions); + return currentToNewObjectPositions; + } + private HashMap updateHashMapObject(Translation2d objectUpdate, Translation2d closestObjectToObjectUpdate, HashMap objectsToUpdate) { if (objectsToUpdate.containsKey(closestObjectToObjectUpdate)) { final Translation2d closestKnownObjectPreviousUpdate = objectsToUpdate.get(closestObjectToObjectUpdate); @@ -195,17 +157,32 @@ private HashMap updateHashMapObject(Translation2d return objectsToUpdate; } - private boolean isObjectsDistanceWithinTolerance(Translation2d firstObject, Translation2d secondObject) { - if (firstObject == null || secondObject == null) - return false; - return firstObject.getDistance(secondObject) < ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; + private boolean isObjectNew(Translation2d object) { + if (objectPositionsToTimeStamp.isEmpty()) + return true; + return object.getDistance(getClosestKnownObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } - private boolean isObjectNew(Translation2d object, HashMap currentToNewObjectPositions) { - if (objectPositionsToTimeStamp.isEmpty() && currentToNewObjectPositions.isEmpty()) - return true; - return object.getDistance(getClosestKnownObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS - && !isObjectsDistanceWithinTolerance(object, getClosestObjectFromSetToPosition(object, currentToNewObjectPositions.keySet())); + private Translation2d getClosestKnownObjectToPosition(Translation2d position) { + return getClosestObjectFromSetToPosition(position, objectPositionsToTimeStamp.keySet()); + } + + private Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set objects) { + if (objects.isEmpty()) + return null; + final Translation2d[] objectsTranslations = objects.toArray(Translation2d[]::new); + Translation2d closestObjectTranslation = objectsTranslations[0]; + double closestObjectDistance = position.getDistance(closestObjectTranslation); + + for (int i = 1; i < objectsTranslations.length; i++) { + final Translation2d currentObjectTranslation = objectsTranslations[i]; + final double currentObjectDistance = position.getDistance(currentObjectTranslation); + if (currentObjectDistance < closestObjectDistance) { + closestObjectDistance = currentObjectDistance; + closestObjectTranslation = currentObjectTranslation; + } + } + return closestObjectTranslation; } private void removeOldObjects() { From b0fe9e020953c5388b276a235ba768683eee73bc Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 16:08:51 +0200 Subject: [PATCH 44/68] changed functions to void --- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index e0e298e7..ac955881 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -141,11 +141,11 @@ private HashMap updateObjectPosition(Translation2d if (isObjectNew(object)) currentToNewObjectPositions.put(object, object); else - currentToNewObjectPositions = updateHashMapObject(object, closestObjectToTargetObject, currentToNewObjectPositions); + updateHashMapObject(object, closestObjectToTargetObject, currentToNewObjectPositions); return currentToNewObjectPositions; } - private HashMap updateHashMapObject(Translation2d objectUpdate, Translation2d closestObjectToObjectUpdate, HashMap objectsToUpdate) { + private void updateHashMapObject(Translation2d objectUpdate, Translation2d closestObjectToObjectUpdate, HashMap objectsToUpdate) { if (objectsToUpdate.containsKey(closestObjectToObjectUpdate)) { final Translation2d closestKnownObjectPreviousUpdate = objectsToUpdate.get(closestObjectToObjectUpdate); if (objectUpdate.getDistance(closestObjectToObjectUpdate) < closestKnownObjectPreviousUpdate.getDistance(closestObjectToObjectUpdate)) { @@ -154,7 +154,6 @@ private HashMap updateHashMapObject(Translation2d } } else objectsToUpdate.put(closestObjectToObjectUpdate, objectUpdate); - return objectsToUpdate; } private boolean isObjectNew(Translation2d object) { From d6cff5a9b5699bea45d2767970d41d46c4dbb6d5 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 16:14:18 +0200 Subject: [PATCH 45/68] added object checking before removing them --- .../misc/objectDetection/ObjectPoseEstimator.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index ac955881..3ab93471 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -61,6 +61,8 @@ public ArrayList getObjectsOnField() { */ public void removeClosestObjectToRobot() { final Translation2d closestObject = getClosestObjectToRobot(); + if (closestObject == null) + return; removeObject(closestObject); } @@ -71,7 +73,10 @@ public void removeClosestObjectToRobot() { */ public void removeClosestObjectToIntake(Transform2d intakeTransform) { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - removeObject(getClosestKnownObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation())); + final Translation2d closestObjectToIntake = getClosestKnownObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation()); + if (closestObjectToIntake == null) + return; + removeObject(closestObjectToIntake); } /** @@ -80,12 +85,13 @@ public void removeClosestObjectToIntake(Transform2d intakeTransform) { * @param fieldRelativePose the pose to which the removed object is closest */ public void removeClosestObjectToPose(Pose2d fieldRelativePose) { - final Translation2d closestObject = getClosestKnownObjectToPosition(fieldRelativePose.getTranslation()); - removeObject(closestObject); + removeClosestObjectToPosition(fieldRelativePose.getTranslation()); } public void removeClosestObjectToPosition(Translation2d position) { final Translation2d closestObject = getClosestKnownObjectToPosition(position); + if (closestObject == null) + return; removeObject(closestObject); } From daa5a01d56f53fa3d017030428568dd2f083c08e Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 16:18:43 +0200 Subject: [PATCH 46/68] made functions void --- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 3ab93471..51ccad1c 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -126,12 +126,12 @@ public Translation2d getClosestObjectToRobot() { } private void updateObjectPositions() { - HashMap currentToNewObjectPositions = new HashMap<>(); + HashMap trackedObjectsToUpdatedPositions = new HashMap<>(); for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) - currentToNewObjectPositions = updateObjectPosition(visibleObject, currentToNewObjectPositions); + updateObjectPosition(visibleObject, trackedObjectsToUpdatedPositions); - mergeObjectsIntoHashmap(currentToNewObjectPositions); + mergeObjectsIntoHashmap(trackedObjectsToUpdatedPositions); } private void mergeObjectsIntoHashmap(HashMap currentToNewObjectPositions) { @@ -141,14 +141,13 @@ private void mergeObjectsIntoHashmap(HashMap curre currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimeStamp.put(object, currentTimestamp)); } - private HashMap updateObjectPosition(Translation2d object, HashMap currentToNewObjectPositions) { + private void updateObjectPosition(Translation2d object, HashMap currentToNewObjectPositions) { final Translation2d closestObjectToTargetObject = getClosestKnownObjectToPosition(object); if (isObjectNew(object)) currentToNewObjectPositions.put(object, object); else updateHashMapObject(object, closestObjectToTargetObject, currentToNewObjectPositions); - return currentToNewObjectPositions; } private void updateHashMapObject(Translation2d objectUpdate, Translation2d closestObjectToObjectUpdate, HashMap objectsToUpdate) { From ddd468c7d42fa5d2925f4e1a06101f114cb192de Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 16:37:49 +0200 Subject: [PATCH 47/68] added javadoc for class hashmap --- .../objectDetection/ObjectPoseEstimator.java | 48 +++++++++++-------- 1 file changed, 28 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 51ccad1c..f67780a0 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -18,7 +18,10 @@ public class ObjectPoseEstimator extends SubsystemBase { private final double deletionThresholdSeconds; private final SimulatedGamePieceConstants.GamePieceType gamePieceType; private final ObjectDetectionCamera camera; - private final HashMap objectPositionsToTimeStamp; + /** + * Holds the position of each detected object and when it was detected. + */ + private final HashMap objectPositionsToTimestamp; /** * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. @@ -33,7 +36,7 @@ public ObjectPoseEstimator(double deletionThresholdSeconds, this.deletionThresholdSeconds = deletionThresholdSeconds; this.gamePieceType = gamePieceType; this.camera = camera; - this.objectPositionsToTimeStamp = new HashMap<>(); + this.objectPositionsToTimestamp = new HashMap<>(); } /** @@ -53,7 +56,7 @@ public void periodic() { * @return a list of Translation2d representing the positions of objects on the field */ public ArrayList getObjectsOnField() { - return new ArrayList<>(objectPositionsToTimeStamp.keySet()); + return new ArrayList<>(objectPositionsToTimestamp.keySet()); } /** @@ -101,7 +104,7 @@ public void removeClosestObjectToPosition(Translation2d 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) { - objectPositionsToTimeStamp.remove(objectPosition); + objectPositionsToTimestamp.remove(objectPosition); } /** @@ -110,7 +113,7 @@ public void removeObject(Translation2d objectPosition) { * @return if there are objects stored in the poseEstimator */ public boolean hasObjects() { - return !getObjectsOnField().isEmpty(); + return !objectPositionsToTimestamp.isEmpty(); } /** @@ -137,38 +140,43 @@ private void updateObjectPositions() { private void mergeObjectsIntoHashmap(HashMap currentToNewObjectPositions) { final double currentTimestamp = Timer.getTimestamp(); - currentToNewObjectPositions.keySet().forEach(objectPositionsToTimeStamp::remove); - currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimeStamp.put(object, currentTimestamp)); + currentToNewObjectPositions.keySet().forEach(objectPositionsToTimestamp::remove); + currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimestamp.put(object, currentTimestamp)); } - private void updateObjectPosition(Translation2d object, HashMap currentToNewObjectPositions) { + private void updateObjectPosition(Translation2d object, HashMap objectsToUpdatedPositions) { final Translation2d closestObjectToTargetObject = getClosestKnownObjectToPosition(object); if (isObjectNew(object)) - currentToNewObjectPositions.put(object, object); + objectsToUpdatedPositions.put(object, object); else - updateHashMapObject(object, closestObjectToTargetObject, currentToNewObjectPositions); + updateHashMapObject(object, closestObjectToTargetObject, objectsToUpdatedPositions); } - private void updateHashMapObject(Translation2d objectUpdate, Translation2d closestObjectToObjectUpdate, HashMap objectsToUpdate) { - if (objectsToUpdate.containsKey(closestObjectToObjectUpdate)) { - final Translation2d closestKnownObjectPreviousUpdate = objectsToUpdate.get(closestObjectToObjectUpdate); - if (objectUpdate.getDistance(closestObjectToObjectUpdate) < closestKnownObjectPreviousUpdate.getDistance(closestObjectToObjectUpdate)) { - objectsToUpdate.replace(closestObjectToObjectUpdate, objectUpdate); - objectsToUpdate.put(closestKnownObjectPreviousUpdate, closestKnownObjectPreviousUpdate); + private void updateHashMapObject(Translation2d objectUpdate, Translation2d objectToUpdate, HashMap objectsToUpdatedPositions) { + if (objectsToUpdatedPositions.containsKey(objectToUpdate)) { + final Translation2d existingObjectUpdate = objectsToUpdatedPositions.get(objectToUpdate); + if (shouldReplaceExistingUpdateWithNewUpdate(objectUpdate, existingObjectUpdate, objectToUpdate)) { + objectsToUpdatedPositions.replace(objectToUpdate, objectUpdate); + objectsToUpdatedPositions.put(existingObjectUpdate, existingObjectUpdate); } } else - objectsToUpdate.put(closestObjectToObjectUpdate, objectUpdate); + objectsToUpdatedPositions.put(objectToUpdate, objectUpdate); + } + + private boolean shouldReplaceExistingUpdateWithNewUpdate(Translation2d newUpdate, Translation2d existingUpdate, Translation2d objectToUpdate) { + return newUpdate.getDistance(objectToUpdate) < + existingUpdate.getDistance(objectToUpdate); } private boolean isObjectNew(Translation2d object) { - if (objectPositionsToTimeStamp.isEmpty()) + if (objectPositionsToTimestamp.isEmpty()) return true; return object.getDistance(getClosestKnownObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } private Translation2d getClosestKnownObjectToPosition(Translation2d position) { - return getClosestObjectFromSetToPosition(position, objectPositionsToTimeStamp.keySet()); + return getClosestObjectFromSetToPosition(position, objectPositionsToTimestamp.keySet()); } private Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set objects) { @@ -190,7 +198,7 @@ private Translation2d getClosestObjectFromSetToPosition(Translation2d position, } private void removeOldObjects() { - objectPositionsToTimeStamp.entrySet().removeIf(entry -> isTooOld(entry.getValue())); + objectPositionsToTimestamp.entrySet().removeIf(entry -> isTooOld(entry.getValue())); } private boolean isTooOld(double timestamp) { From ebd93d862c38c592c892ea3c32f2569f39cb36c1 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 16:57:21 +0200 Subject: [PATCH 48/68] fixed javadoc --- .../objectDetection/ObjectPoseEstimator.java | 36 ++++++++++--------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index f67780a0..719198a3 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -117,19 +117,16 @@ public boolean hasObjects() { } /** - * Gets the position of the closest object on the field from the 3D rotation of the object 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. + * Gets the position of the closest object to the robot. * - * @return the best object's 2D position on the field (z is assumed to be 0) + * @return the closest object to the robot */ public Translation2d getClosestObjectToRobot() { return getClosestKnownObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); } private void updateObjectPositions() { - HashMap trackedObjectsToUpdatedPositions = new HashMap<>(); + final HashMap trackedObjectsToUpdatedPositions = new HashMap<>(); for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) updateObjectPosition(visibleObject, trackedObjectsToUpdatedPositions); @@ -153,18 +150,25 @@ private void updateObjectPosition(Translation2d object, HashMap objectsToUpdatedPositions) { - if (objectsToUpdatedPositions.containsKey(objectToUpdate)) { - final Translation2d existingObjectUpdate = objectsToUpdatedPositions.get(objectToUpdate); - if (shouldReplaceExistingUpdateWithNewUpdate(objectUpdate, existingObjectUpdate, objectToUpdate)) { - objectsToUpdatedPositions.replace(objectToUpdate, objectUpdate); - objectsToUpdatedPositions.put(existingObjectUpdate, existingObjectUpdate); - } - } else - objectsToUpdatedPositions.put(objectToUpdate, objectUpdate); + private void updateHashMapObject(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap objectsToUpdatedPositions) { + if (objectsToUpdatedPositions.containsKey(objectToUpdate)) + addClosestUpdatedPositionToHashMap(objectsUpdatedPosition, objectToUpdate, objectsToUpdatedPositions); + else + objectsToUpdatedPositions.put(objectToUpdate, objectsUpdatedPosition); + } + + private void addClosestUpdatedPositionToHashMap(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap objectsToUpdatedPositions) { + final Translation2d existingUpdatedPosition = objectsToUpdatedPositions.get(objectToUpdate); + if (shouldReplaceExistingUpdateWithNewUpdate(objectsUpdatedPosition, existingUpdatedPosition, objectToUpdate)) { + objectsToUpdatedPositions.replace(objectToUpdate, objectsUpdatedPosition); + objectsToUpdatedPositions.put(existingUpdatedPosition, existingUpdatedPosition); + return; + } + objectsToUpdatedPositions.put(objectsUpdatedPosition, objectToUpdate); } - private boolean shouldReplaceExistingUpdateWithNewUpdate(Translation2d newUpdate, Translation2d existingUpdate, Translation2d objectToUpdate) { + private boolean shouldReplaceExistingUpdateWithNewUpdate(Translation2d newUpdate, Translation2d + existingUpdate, Translation2d objectToUpdate) { return newUpdate.getDistance(objectToUpdate) < existingUpdate.getDistance(objectToUpdate); } From 12ecdd29ea6c3605807691fae674bc1a9666e5b1 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 17:00:09 +0200 Subject: [PATCH 49/68] added javadoc --- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 719198a3..b7f655bc 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -91,6 +91,11 @@ public void removeClosestObjectToPose(Pose2d fieldRelativePose) { removeClosestObjectToPosition(fieldRelativePose.getTranslation()); } + /** + * Removes the closest object to a given position from the list of objects in the pose estimator. + * + * @param position the position to which the removed object is closest + */ public void removeClosestObjectToPosition(Translation2d position) { final Translation2d closestObject = getClosestKnownObjectToPosition(position); if (closestObject == null) From c58b5f71ba7a62babb7648ff04e414572311d26f Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 17:37:54 +0200 Subject: [PATCH 50/68] added function for updating position of discarded update --- .../objectDetection/ObjectPoseEstimator.java | 26 +++++++++++++++---- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index b7f655bc..cc44ecea 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -12,12 +12,14 @@ 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; + private final Set excludedKnownObjects = new HashSet<>(); /** * Holds the position of each detected object and when it was detected. */ @@ -45,7 +47,7 @@ public ObjectPoseEstimator(double deletionThresholdSeconds, */ @Override public void periodic() { - updateObjectPositions(); + updateObjectsPositions(); removeOldObjects(); Logger.recordOutput("ObjectPoseEstimator/knownObjectPositions", getObjectsOnField().toArray(Translation2d[]::new)); } @@ -130,11 +132,13 @@ public Translation2d getClosestObjectToRobot() { return getClosestKnownObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); } - private void updateObjectPositions() { + private void updateObjectsPositions() { final HashMap trackedObjectsToUpdatedPositions = new HashMap<>(); - for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) + for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { + excludedKnownObjects.clear(); updateObjectPosition(visibleObject, trackedObjectsToUpdatedPositions); + } mergeObjectsIntoHashmap(trackedObjectsToUpdatedPositions); } @@ -147,7 +151,7 @@ private void mergeObjectsIntoHashmap(HashMap curre } private void updateObjectPosition(Translation2d object, HashMap objectsToUpdatedPositions) { - final Translation2d closestObjectToTargetObject = getClosestKnownObjectToPosition(object); + final Translation2d closestObjectToTargetObject = getNextClosestKnownObjectToPosition(object, (Translation2d) excludedKnownObjects); if (isObjectNew(object)) objectsToUpdatedPositions.put(object, object); @@ -166,12 +170,17 @@ private void addClosestUpdatedPositionToHashMap(Translation2d objectsUpdatedPosi final Translation2d existingUpdatedPosition = objectsToUpdatedPositions.get(objectToUpdate); if (shouldReplaceExistingUpdateWithNewUpdate(objectsUpdatedPosition, existingUpdatedPosition, objectToUpdate)) { objectsToUpdatedPositions.replace(objectToUpdate, objectsUpdatedPosition); - objectsToUpdatedPositions.put(existingUpdatedPosition, existingUpdatedPosition); + updatePositionOfDiscardedObjectUpdate(existingUpdatedPosition, objectToUpdate, objectsToUpdatedPositions); return; } objectsToUpdatedPositions.put(objectsUpdatedPosition, objectToUpdate); } + private void updatePositionOfDiscardedObjectUpdate(Translation2d discardedUpdate, Translation2d previousClosestObject, HashMap objectsToUpdatedPositions) { + updateObjectPosition(discardedUpdate, objectsToUpdatedPositions); + excludedKnownObjects.add(previousClosestObject); + } + private boolean shouldReplaceExistingUpdateWithNewUpdate(Translation2d newUpdate, Translation2d existingUpdate, Translation2d objectToUpdate) { return newUpdate.getDistance(objectToUpdate) < @@ -184,6 +193,13 @@ private boolean isObjectNew(Translation2d object) { return object.getDistance(getClosestKnownObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } + private Translation2d getNextClosestKnownObjectToPosition(Translation2d position, Translation2d... objectsToExclude) { + Set candidateObjects = objectPositionsToTimestamp.keySet(); + for (Translation2d excludedObject : objectsToExclude) + candidateObjects.remove(excludedObject); + return getClosestObjectFromSetToPosition(position, candidateObjects); + } + private Translation2d getClosestKnownObjectToPosition(Translation2d position) { return getClosestObjectFromSetToPosition(position, objectPositionsToTimestamp.keySet()); } From 4e0068c61a6eaedd9cf226ce6a40dccdeab89908 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 17:49:45 +0200 Subject: [PATCH 51/68] fix logic issue --- .../misc/objectDetection/ObjectPoseEstimator.java | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index cc44ecea..4493d0c6 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -115,7 +115,7 @@ public void removeObject(Translation2d objectPosition) { } /** - * determines whether any objects are stored in the poseEstimator. + * Determines whether any objects are stored in the poseEstimator. * * @return if there are objects stored in the poseEstimator */ @@ -151,7 +151,7 @@ private void mergeObjectsIntoHashmap(HashMap curre } private void updateObjectPosition(Translation2d object, HashMap objectsToUpdatedPositions) { - final Translation2d closestObjectToTargetObject = getNextClosestKnownObjectToPosition(object, (Translation2d) excludedKnownObjects); + final Translation2d closestObjectToTargetObject = getNextClosestKnownObjectToPosition(object); if (isObjectNew(object)) objectsToUpdatedPositions.put(object, object); @@ -173,7 +173,7 @@ private void addClosestUpdatedPositionToHashMap(Translation2d objectsUpdatedPosi updatePositionOfDiscardedObjectUpdate(existingUpdatedPosition, objectToUpdate, objectsToUpdatedPositions); return; } - objectsToUpdatedPositions.put(objectsUpdatedPosition, objectToUpdate); + updatePositionOfDiscardedObjectUpdate(objectsUpdatedPosition, objectToUpdate, objectsToUpdatedPositions); } private void updatePositionOfDiscardedObjectUpdate(Translation2d discardedUpdate, Translation2d previousClosestObject, HashMap objectsToUpdatedPositions) { @@ -181,8 +181,7 @@ private void updatePositionOfDiscardedObjectUpdate(Translation2d discardedUpdate excludedKnownObjects.add(previousClosestObject); } - private boolean shouldReplaceExistingUpdateWithNewUpdate(Translation2d newUpdate, Translation2d - existingUpdate, Translation2d objectToUpdate) { + private boolean shouldReplaceExistingUpdateWithNewUpdate(Translation2d newUpdate, Translation2d existingUpdate, Translation2d objectToUpdate) { return newUpdate.getDistance(objectToUpdate) < existingUpdate.getDistance(objectToUpdate); } @@ -193,9 +192,9 @@ private boolean isObjectNew(Translation2d object) { return object.getDistance(getClosestKnownObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } - private Translation2d getNextClosestKnownObjectToPosition(Translation2d position, Translation2d... objectsToExclude) { - Set candidateObjects = objectPositionsToTimestamp.keySet(); - for (Translation2d excludedObject : objectsToExclude) + private Translation2d getNextClosestKnownObjectToPosition(Translation2d position) { + Set candidateObjects = new HashSet<>(objectPositionsToTimestamp.keySet()); + for (Translation2d excludedObject : excludedKnownObjects) candidateObjects.remove(excludedObject); return getClosestObjectFromSetToPosition(position, candidateObjects); } From 3ef33a79f494f98ca4e1c8c3d2869033353aa973 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 18:14:23 +0200 Subject: [PATCH 52/68] fixed logic issue --- .../objectDetection/ObjectPoseEstimator.java | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 4493d0c6..d713f51a 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -137,7 +137,8 @@ private void updateObjectsPositions() { for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { excludedKnownObjects.clear(); - updateObjectPosition(visibleObject, trackedObjectsToUpdatedPositions); + final Translation2d closestKnownObjectToVisibleObject = getClosestKnownObjectToPosition(visibleObject); + updateObjectPosition(visibleObject, closestKnownObjectToVisibleObject, trackedObjectsToUpdatedPositions); } mergeObjectsIntoHashmap(trackedObjectsToUpdatedPositions); @@ -150,10 +151,8 @@ private void mergeObjectsIntoHashmap(HashMap curre currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimestamp.put(object, currentTimestamp)); } - private void updateObjectPosition(Translation2d object, HashMap objectsToUpdatedPositions) { - final Translation2d closestObjectToTargetObject = getNextClosestKnownObjectToPosition(object); - - if (isObjectNew(object)) + private void updateObjectPosition(Translation2d object, Translation2d closestObjectToTargetObject, HashMap objectsToUpdatedPositions) { + if (isObjectNew(object) || closestObjectToTargetObject == null) objectsToUpdatedPositions.put(object, object); else updateHashMapObject(object, closestObjectToTargetObject, objectsToUpdatedPositions); @@ -176,9 +175,15 @@ private void addClosestUpdatedPositionToHashMap(Translation2d objectsUpdatedPosi updatePositionOfDiscardedObjectUpdate(objectsUpdatedPosition, objectToUpdate, objectsToUpdatedPositions); } - private void updatePositionOfDiscardedObjectUpdate(Translation2d discardedUpdate, Translation2d previousClosestObject, HashMap objectsToUpdatedPositions) { - updateObjectPosition(discardedUpdate, objectsToUpdatedPositions); + private void updatePositionOfDiscardedObjectUpdate(Translation2d discardedObjectUpdate, Translation2d previousClosestObject, HashMap objectsToUpdatedPositions) { excludedKnownObjects.add(previousClosestObject); + Translation2d nextClosestObjectToDiscardedObjectUpdate = getNextClosestKnownObjectToPosition(discardedObjectUpdate); + if (discardedObjectUpdate.getDistance(nextClosestObjectToDiscardedObjectUpdate) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS + || nextClosestObjectToDiscardedObjectUpdate == null) { + objectsToUpdatedPositions.put(discardedObjectUpdate, discardedObjectUpdate); + return; + } + updateObjectPosition(discardedObjectUpdate, nextClosestObjectToDiscardedObjectUpdate, objectsToUpdatedPositions); } private boolean shouldReplaceExistingUpdateWithNewUpdate(Translation2d newUpdate, Translation2d existingUpdate, Translation2d objectToUpdate) { From 8104499baf3524416ddbfe15256029450d8df386 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 18:19:40 +0200 Subject: [PATCH 53/68] fix order so no crash --- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index d713f51a..6118467f 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -178,8 +178,8 @@ private void addClosestUpdatedPositionToHashMap(Translation2d objectsUpdatedPosi private void updatePositionOfDiscardedObjectUpdate(Translation2d discardedObjectUpdate, Translation2d previousClosestObject, HashMap objectsToUpdatedPositions) { excludedKnownObjects.add(previousClosestObject); Translation2d nextClosestObjectToDiscardedObjectUpdate = getNextClosestKnownObjectToPosition(discardedObjectUpdate); - if (discardedObjectUpdate.getDistance(nextClosestObjectToDiscardedObjectUpdate) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS - || nextClosestObjectToDiscardedObjectUpdate == null) { + if (nextClosestObjectToDiscardedObjectUpdate == null || + discardedObjectUpdate.getDistance(nextClosestObjectToDiscardedObjectUpdate) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) { objectsToUpdatedPositions.put(discardedObjectUpdate, discardedObjectUpdate); return; } @@ -199,8 +199,7 @@ private boolean isObjectNew(Translation2d object) { private Translation2d getNextClosestKnownObjectToPosition(Translation2d position) { Set candidateObjects = new HashSet<>(objectPositionsToTimestamp.keySet()); - for (Translation2d excludedObject : excludedKnownObjects) - candidateObjects.remove(excludedObject); + candidateObjects.removeAll(excludedKnownObjects); return getClosestObjectFromSetToPosition(position, candidateObjects); } From 031c69cf53176803253e357f7831de280ca9d40b Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 18:55:41 +0200 Subject: [PATCH 54/68] clean --- .../objectDetection/ObjectPoseEstimator.java | 23 +++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 6118467f..5e1fd15c 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -19,7 +19,6 @@ public class ObjectPoseEstimator extends SubsystemBase { private final double deletionThresholdSeconds; private final SimulatedGamePieceConstants.GamePieceType gamePieceType; private final ObjectDetectionCamera camera; - private final Set excludedKnownObjects = new HashSet<>(); /** * Holds the position of each detected object and when it was detected. */ @@ -136,7 +135,6 @@ private void updateObjectsPositions() { final HashMap trackedObjectsToUpdatedPositions = new HashMap<>(); for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { - excludedKnownObjects.clear(); final Translation2d closestKnownObjectToVisibleObject = getClosestKnownObjectToPosition(visibleObject); updateObjectPosition(visibleObject, closestKnownObjectToVisibleObject, trackedObjectsToUpdatedPositions); } @@ -159,31 +157,32 @@ private void updateObjectPosition(Translation2d object, Translation2d closestObj } private void updateHashMapObject(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap objectsToUpdatedPositions) { - if (objectsToUpdatedPositions.containsKey(objectToUpdate)) - addClosestUpdatedPositionToHashMap(objectsUpdatedPosition, objectToUpdate, objectsToUpdatedPositions); - else + if (objectsToUpdatedPositions.containsKey(objectToUpdate)) { + final Set excludedKnownObjects = new HashSet<>(); + addClosestUpdatedPositionToHashMap(objectsUpdatedPosition, objectToUpdate, objectsToUpdatedPositions, excludedKnownObjects); + } else objectsToUpdatedPositions.put(objectToUpdate, objectsUpdatedPosition); } - private void addClosestUpdatedPositionToHashMap(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap objectsToUpdatedPositions) { + private void addClosestUpdatedPositionToHashMap(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap objectsToUpdatedPositions, Set excludedKnownObjects) { final Translation2d existingUpdatedPosition = objectsToUpdatedPositions.get(objectToUpdate); if (shouldReplaceExistingUpdateWithNewUpdate(objectsUpdatedPosition, existingUpdatedPosition, objectToUpdate)) { objectsToUpdatedPositions.replace(objectToUpdate, objectsUpdatedPosition); - updatePositionOfDiscardedObjectUpdate(existingUpdatedPosition, objectToUpdate, objectsToUpdatedPositions); + updatePositionOfDiscardedObjectUpdate(existingUpdatedPosition, objectToUpdate, objectsToUpdatedPositions, excludedKnownObjects); return; } - updatePositionOfDiscardedObjectUpdate(objectsUpdatedPosition, objectToUpdate, objectsToUpdatedPositions); + updatePositionOfDiscardedObjectUpdate(objectsUpdatedPosition, objectToUpdate, objectsToUpdatedPositions, excludedKnownObjects); } - private void updatePositionOfDiscardedObjectUpdate(Translation2d discardedObjectUpdate, Translation2d previousClosestObject, HashMap objectsToUpdatedPositions) { + private void updatePositionOfDiscardedObjectUpdate(Translation2d discardedObjectUpdate, Translation2d previousClosestObject, HashMap objectsToUpdatedPositions, Set excludedKnownObjects) { excludedKnownObjects.add(previousClosestObject); - Translation2d nextClosestObjectToDiscardedObjectUpdate = getNextClosestKnownObjectToPosition(discardedObjectUpdate); + final Translation2d nextClosestObjectToDiscardedObjectUpdate = getNextClosestKnownObjectToPosition(discardedObjectUpdate, excludedKnownObjects); if (nextClosestObjectToDiscardedObjectUpdate == null || discardedObjectUpdate.getDistance(nextClosestObjectToDiscardedObjectUpdate) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) { objectsToUpdatedPositions.put(discardedObjectUpdate, discardedObjectUpdate); return; } - updateObjectPosition(discardedObjectUpdate, nextClosestObjectToDiscardedObjectUpdate, objectsToUpdatedPositions); + addClosestUpdatedPositionToHashMap(discardedObjectUpdate, nextClosestObjectToDiscardedObjectUpdate, objectsToUpdatedPositions, excludedKnownObjects); } private boolean shouldReplaceExistingUpdateWithNewUpdate(Translation2d newUpdate, Translation2d existingUpdate, Translation2d objectToUpdate) { @@ -197,7 +196,7 @@ private boolean isObjectNew(Translation2d object) { return object.getDistance(getClosestKnownObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } - private Translation2d getNextClosestKnownObjectToPosition(Translation2d position) { + private Translation2d getNextClosestKnownObjectToPosition(Translation2d position, Set excludedKnownObjects) { Set candidateObjects = new HashSet<>(objectPositionsToTimestamp.keySet()); candidateObjects.removeAll(excludedKnownObjects); return getClosestObjectFromSetToPosition(position, candidateObjects); From 34991275fb9d48e597b3a93ff49d5ba87f442d61 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 19:08:58 +0200 Subject: [PATCH 55/68] rename --- .../objectDetection/ObjectPoseEstimator.java | 64 +++++++++---------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 5e1fd15c..4c66eaf0 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -46,7 +46,7 @@ public ObjectPoseEstimator(double deletionThresholdSeconds, */ @Override public void periodic() { - updateObjectsPositions(); + updateTrackedObjectsPositions(); removeOldObjects(); Logger.recordOutput("ObjectPoseEstimator/knownObjectPositions", getObjectsOnField().toArray(Translation2d[]::new)); } @@ -77,7 +77,7 @@ public void removeClosestObjectToRobot() { */ public void removeClosestObjectToIntake(Transform2d intakeTransform) { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - final Translation2d closestObjectToIntake = getClosestKnownObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation()); + final Translation2d closestObjectToIntake = getClosestTrackedObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation()); if (closestObjectToIntake == null) return; removeObject(closestObjectToIntake); @@ -98,7 +98,7 @@ public void removeClosestObjectToPose(Pose2d fieldRelativePose) { * @param position the position to which the removed object is closest */ public void removeClosestObjectToPosition(Translation2d position) { - final Translation2d closestObject = getClosestKnownObjectToPosition(position); + final Translation2d closestObject = getClosestTrackedObjectToPosition(position); if (closestObject == null) return; removeObject(closestObject); @@ -128,72 +128,72 @@ public boolean hasObjects() { * @return the closest object to the robot */ public Translation2d getClosestObjectToRobot() { - return getClosestKnownObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); + return getClosestTrackedObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); } - private void updateObjectsPositions() { + private void updateTrackedObjectsPositions() { final HashMap trackedObjectsToUpdatedPositions = new HashMap<>(); for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { - final Translation2d closestKnownObjectToVisibleObject = getClosestKnownObjectToPosition(visibleObject); - updateObjectPosition(visibleObject, closestKnownObjectToVisibleObject, trackedObjectsToUpdatedPositions); + updateObjectPosition(visibleObject, trackedObjectsToUpdatedPositions); } - mergeObjectsIntoHashmap(trackedObjectsToUpdatedPositions); + commitObjectUpdates(trackedObjectsToUpdatedPositions); } - private void mergeObjectsIntoHashmap(HashMap currentToNewObjectPositions) { + private void commitObjectUpdates(HashMap currentToNewObjectPositions) { final double currentTimestamp = Timer.getTimestamp(); currentToNewObjectPositions.keySet().forEach(objectPositionsToTimestamp::remove); currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimestamp.put(object, currentTimestamp)); } - private void updateObjectPosition(Translation2d object, Translation2d closestObjectToTargetObject, HashMap objectsToUpdatedPositions) { - if (isObjectNew(object) || closestObjectToTargetObject == null) - objectsToUpdatedPositions.put(object, object); + private void updateObjectPosition(Translation2d object, HashMap trackedObjectsToUpdatedPositions) { + final Translation2d nearestTrackedObject = getClosestTrackedObjectToPosition(object); + if (isObjectNew(object)) + trackedObjectsToUpdatedPositions.put(object, object); else - updateHashMapObject(object, closestObjectToTargetObject, objectsToUpdatedPositions); + assignUpdateToTrackedObject(object, nearestTrackedObject, trackedObjectsToUpdatedPositions); } - private void updateHashMapObject(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap objectsToUpdatedPositions) { - if (objectsToUpdatedPositions.containsKey(objectToUpdate)) { + private void assignUpdateToTrackedObject(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap trackedObjectsToUpdatedPositions) { + if (trackedObjectsToUpdatedPositions.containsKey(objectToUpdate)) { final Set excludedKnownObjects = new HashSet<>(); - addClosestUpdatedPositionToHashMap(objectsUpdatedPosition, objectToUpdate, objectsToUpdatedPositions, excludedKnownObjects); + addClosestUpdatedPositionToHashMap(objectsUpdatedPosition, objectToUpdate, trackedObjectsToUpdatedPositions, excludedKnownObjects); } else - objectsToUpdatedPositions.put(objectToUpdate, objectsUpdatedPosition); + trackedObjectsToUpdatedPositions.put(objectToUpdate, objectsUpdatedPosition); } - private void addClosestUpdatedPositionToHashMap(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap objectsToUpdatedPositions, Set excludedKnownObjects) { - final Translation2d existingUpdatedPosition = objectsToUpdatedPositions.get(objectToUpdate); - if (shouldReplaceExistingUpdateWithNewUpdate(objectsUpdatedPosition, existingUpdatedPosition, objectToUpdate)) { - objectsToUpdatedPositions.replace(objectToUpdate, objectsUpdatedPosition); - updatePositionOfDiscardedObjectUpdate(existingUpdatedPosition, objectToUpdate, objectsToUpdatedPositions, excludedKnownObjects); + private void addClosestUpdatedPositionToHashMap(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap trackedObjectsToUpdatedPositions, Set excludedKnownObjects) { + final Translation2d existingUpdatedPosition = trackedObjectsToUpdatedPositions.get(objectToUpdate); + if (isNewObjectUpdateCloser(objectsUpdatedPosition, existingUpdatedPosition, objectToUpdate)) { + trackedObjectsToUpdatedPositions.replace(objectToUpdate, objectsUpdatedPosition); + reassignDiscardedObjectUpdate(existingUpdatedPosition, objectToUpdate, trackedObjectsToUpdatedPositions, excludedKnownObjects); return; } - updatePositionOfDiscardedObjectUpdate(objectsUpdatedPosition, objectToUpdate, objectsToUpdatedPositions, excludedKnownObjects); + reassignDiscardedObjectUpdate(objectsUpdatedPosition, objectToUpdate, trackedObjectsToUpdatedPositions, excludedKnownObjects); } - private void updatePositionOfDiscardedObjectUpdate(Translation2d discardedObjectUpdate, Translation2d previousClosestObject, HashMap objectsToUpdatedPositions, Set excludedKnownObjects) { - excludedKnownObjects.add(previousClosestObject); + private void reassignDiscardedObjectUpdate(Translation2d discardedObjectUpdate, Translation2d previousClosestTrackedObject, HashMap trackedObjectsToUpdatedPositions, Set excludedKnownObjects) { + excludedKnownObjects.add(previousClosestTrackedObject); final Translation2d nextClosestObjectToDiscardedObjectUpdate = getNextClosestKnownObjectToPosition(discardedObjectUpdate, excludedKnownObjects); if (nextClosestObjectToDiscardedObjectUpdate == null || discardedObjectUpdate.getDistance(nextClosestObjectToDiscardedObjectUpdate) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) { - objectsToUpdatedPositions.put(discardedObjectUpdate, discardedObjectUpdate); + trackedObjectsToUpdatedPositions.put(discardedObjectUpdate, discardedObjectUpdate); return; } - addClosestUpdatedPositionToHashMap(discardedObjectUpdate, nextClosestObjectToDiscardedObjectUpdate, objectsToUpdatedPositions, excludedKnownObjects); + addClosestUpdatedPositionToHashMap(discardedObjectUpdate, nextClosestObjectToDiscardedObjectUpdate, trackedObjectsToUpdatedPositions, excludedKnownObjects); } - private boolean shouldReplaceExistingUpdateWithNewUpdate(Translation2d newUpdate, Translation2d existingUpdate, Translation2d objectToUpdate) { - return newUpdate.getDistance(objectToUpdate) < - existingUpdate.getDistance(objectToUpdate); + private boolean isNewObjectUpdateCloser(Translation2d newUpdate, Translation2d existingUpdate, Translation2d trackedObject) { + return newUpdate.getDistance(trackedObject) < + existingUpdate.getDistance(trackedObject); } private boolean isObjectNew(Translation2d object) { if (objectPositionsToTimestamp.isEmpty()) return true; - return object.getDistance(getClosestKnownObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; + return object.getDistance(getClosestTrackedObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } private Translation2d getNextClosestKnownObjectToPosition(Translation2d position, Set excludedKnownObjects) { @@ -202,7 +202,7 @@ private Translation2d getNextClosestKnownObjectToPosition(Translation2d position return getClosestObjectFromSetToPosition(position, candidateObjects); } - private Translation2d getClosestKnownObjectToPosition(Translation2d position) { + private Translation2d getClosestTrackedObjectToPosition(Translation2d position) { return getClosestObjectFromSetToPosition(position, objectPositionsToTimestamp.keySet()); } From 456f1f8f1d8431cd4ecae24677494d6160faf0f5 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 19:34:02 +0200 Subject: [PATCH 56/68] reduced unnecessary processes --- .../objectDetection/ObjectPoseEstimator.java | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 4c66eaf0..ec565a11 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -22,7 +22,7 @@ public class ObjectPoseEstimator extends SubsystemBase { /** * Holds the position of each detected object and when it was detected. */ - private final HashMap objectPositionsToTimestamp; + private final KDTree2D objectPositionsToTimestamp; /** * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. @@ -132,9 +132,10 @@ public Translation2d getClosestObjectToRobot() { } private void updateTrackedObjectsPositions() { + final Translation2d[] visibleObjects = camera.getObjectsPositionsOnField(gamePieceType); final HashMap trackedObjectsToUpdatedPositions = new HashMap<>(); - for (Translation2d visibleObject : camera.getObjectsPositionsOnField(gamePieceType)) { + for (Translation2d visibleObject : visibleObjects) { updateObjectPosition(visibleObject, trackedObjectsToUpdatedPositions); } @@ -144,7 +145,7 @@ private void updateTrackedObjectsPositions() { private void commitObjectUpdates(HashMap currentToNewObjectPositions) { final double currentTimestamp = Timer.getTimestamp(); - currentToNewObjectPositions.keySet().forEach(objectPositionsToTimestamp::remove); + objectPositionsToTimestamp.keySet().removeAll(currentToNewObjectPositions.keySet()); currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimestamp.put(object, currentTimestamp)); } @@ -209,16 +210,14 @@ private Translation2d getClosestTrackedObjectToPosition(Translation2d position) private Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set objects) { if (objects.isEmpty()) return null; - final Translation2d[] objectsTranslations = objects.toArray(Translation2d[]::new); - Translation2d closestObjectTranslation = objectsTranslations[0]; - double closestObjectDistance = position.getDistance(closestObjectTranslation); + Translation2d closestObjectTranslation = null; + double closestObjectDistance = Double.MAX_VALUE; - for (int i = 1; i < objectsTranslations.length; i++) { - final Translation2d currentObjectTranslation = objectsTranslations[i]; - final double currentObjectDistance = position.getDistance(currentObjectTranslation); + for (Translation2d object : objects) { + final double currentObjectDistance = position.getDistance(object); if (currentObjectDistance < closestObjectDistance) { closestObjectDistance = currentObjectDistance; - closestObjectTranslation = currentObjectTranslation; + closestObjectTranslation = object; } } return closestObjectTranslation; From 83cfa1e5d5411f749a3764cd6acdc041995bc33d Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 21:00:00 +0200 Subject: [PATCH 57/68] Update ObjectPoseEstimator.java --- .../trigon/robot/misc/objectDetection/ObjectPoseEstimator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index ec565a11..2e70fbbf 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -22,7 +22,7 @@ public class ObjectPoseEstimator extends SubsystemBase { /** * Holds the position of each detected object and when it was detected. */ - private final KDTree2D objectPositionsToTimestamp; + private final HashMap objectPositionsToTimestamp; /** * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. From 77e2c55c528062f48a6bcae61976bd04e73988c8 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 21:17:44 +0200 Subject: [PATCH 58/68] Update ObjectPoseEstimator.java --- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 2e70fbbf..8b68c6bf 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -151,7 +151,7 @@ private void commitObjectUpdates(HashMap currentTo private void updateObjectPosition(Translation2d object, HashMap trackedObjectsToUpdatedPositions) { final Translation2d nearestTrackedObject = getClosestTrackedObjectToPosition(object); - if (isObjectNew(object)) + if (isObjectNew(object, nearestTrackedObject)) trackedObjectsToUpdatedPositions.put(object, object); else assignUpdateToTrackedObject(object, nearestTrackedObject, trackedObjectsToUpdatedPositions); @@ -191,10 +191,10 @@ private boolean isNewObjectUpdateCloser(Translation2d newUpdate, Translation2d e existingUpdate.getDistance(trackedObject); } - private boolean isObjectNew(Translation2d object) { + private boolean isObjectNew(Translation2d object, Translation2d nearestTrackedObject) { if (objectPositionsToTimestamp.isEmpty()) return true; - return object.getDistance(getClosestTrackedObjectToPosition(object)) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; + return object.getDistance(nearestTrackedObject) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } private Translation2d getNextClosestKnownObjectToPosition(Translation2d position, Set excludedKnownObjects) { From 3878d709233db0e3197104062d34b4e6692d97b4 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 12 Jan 2026 21:23:34 +0200 Subject: [PATCH 59/68] Update ObjectPoseEstimator.java --- .../trigon/robot/misc/objectDetection/ObjectPoseEstimator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 8b68c6bf..938d2983 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -192,7 +192,7 @@ private boolean isNewObjectUpdateCloser(Translation2d newUpdate, Translation2d e } private boolean isObjectNew(Translation2d object, Translation2d nearestTrackedObject) { - if (objectPositionsToTimestamp.isEmpty()) + if (nearestTrackedObject == null) return true; return object.getDistance(nearestTrackedObject) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; } From bb2bbd39b4cbde5f3df43cdac3f66f694a5cc60f Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 13 Jan 2026 14:13:58 +0200 Subject: [PATCH 60/68] rename and javadoc edits --- .../ObjectDetectionConstants.java | 2 +- .../objectDetection/ObjectPoseEstimator.java | 31 ++++++++++--------- .../ObjectDetectionCamera.java | 23 +++++++------- .../ObjectDetectionCameraIO.java | 4 +-- 4 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java index aa4e647c..242d9f83 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java @@ -4,5 +4,5 @@ public class ObjectDetectionConstants { public static final int NUMBER_OF_GAME_PIECE_TYPES = SimulatedGamePieceConstants.GamePieceType.values().length; - public static final double TRACKED_OBJECT_TOLERANCE_METERS = 0.12; + static final double TRACKED_OBJECT_TOLERANCE_METERS = 0.12; } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 938d2983..eb61efad 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -20,7 +20,7 @@ public class ObjectPoseEstimator extends SubsystemBase { private final SimulatedGamePieceConstants.GamePieceType gamePieceType; private final ObjectDetectionCamera camera; /** - * Holds the position of each detected object and when it was detected. + * Stores the position of each detected object along with the timestamp of when it was detected. */ private final HashMap objectPositionsToTimestamp; @@ -42,7 +42,7 @@ public ObjectPoseEstimator(double deletionThresholdSeconds, /** * Updates the object positions based on the camera detected objects. - * Removes objects that have not been detected for {@link ObjectPoseEstimator#deletionThresholdSeconds}. + * Removes objects that have not been detected for a certain time frame, defined in {@link ObjectPoseEstimator#deletionThresholdSeconds}. */ @Override public void periodic() { @@ -93,7 +93,7 @@ public void removeClosestObjectToPose(Pose2d fieldRelativePose) { } /** - * Removes the closest object to a given position from the list of objects in the pose estimator. + * 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 */ @@ -105,7 +105,8 @@ public void removeClosestObjectToPosition(Translation2d position) { } /** - * Removes a specific object from the list of known objects in the pose estimator. + * 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. */ @@ -114,9 +115,9 @@ public void removeObject(Translation2d objectPosition) { } /** - * Determines whether any objects are stored in the poseEstimator. + * Returns whether any objects are stored in the pose estimator. * - * @return if there are objects stored in the poseEstimator + * @return if there are objects stored in the pose estimator */ public boolean hasObjects() { return !objectPositionsToTimestamp.isEmpty(); @@ -132,17 +133,17 @@ public Translation2d getClosestObjectToRobot() { } private void updateTrackedObjectsPositions() { - final Translation2d[] visibleObjects = camera.getObjectsPositionsOnField(gamePieceType); + final Translation2d[] visibleObjects = camera.getObjectPositionsOnField(gamePieceType); final HashMap trackedObjectsToUpdatedPositions = new HashMap<>(); for (Translation2d visibleObject : visibleObjects) { updateObjectPosition(visibleObject, trackedObjectsToUpdatedPositions); } - commitObjectUpdates(trackedObjectsToUpdatedPositions); + updateObjectUpdates(trackedObjectsToUpdatedPositions); } - private void commitObjectUpdates(HashMap currentToNewObjectPositions) { + private void updateObjectUpdates(HashMap currentToNewObjectPositions) { final double currentTimestamp = Timer.getTimestamp(); objectPositionsToTimestamp.keySet().removeAll(currentToNewObjectPositions.keySet()); @@ -151,7 +152,7 @@ private void commitObjectUpdates(HashMap currentTo private void updateObjectPosition(Translation2d object, HashMap trackedObjectsToUpdatedPositions) { final Translation2d nearestTrackedObject = getClosestTrackedObjectToPosition(object); - if (isObjectNew(object, nearestTrackedObject)) + if (isObjectAlreadyStored(object, nearestTrackedObject)) trackedObjectsToUpdatedPositions.put(object, object); else assignUpdateToTrackedObject(object, nearestTrackedObject, trackedObjectsToUpdatedPositions); @@ -167,7 +168,7 @@ private void assignUpdateToTrackedObject(Translation2d objectsUpdatedPosition, T private void addClosestUpdatedPositionToHashMap(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap trackedObjectsToUpdatedPositions, Set excludedKnownObjects) { final Translation2d existingUpdatedPosition = trackedObjectsToUpdatedPositions.get(objectToUpdate); - if (isNewObjectUpdateCloser(objectsUpdatedPosition, existingUpdatedPosition, objectToUpdate)) { + if (isNewObjectUpdateCloserThanPreviousUpdate(objectsUpdatedPosition, existingUpdatedPosition, objectToUpdate)) { trackedObjectsToUpdatedPositions.replace(objectToUpdate, objectsUpdatedPosition); reassignDiscardedObjectUpdate(existingUpdatedPosition, objectToUpdate, trackedObjectsToUpdatedPositions, excludedKnownObjects); return; @@ -186,12 +187,12 @@ private void reassignDiscardedObjectUpdate(Translation2d discardedObjectUpdate, addClosestUpdatedPositionToHashMap(discardedObjectUpdate, nextClosestObjectToDiscardedObjectUpdate, trackedObjectsToUpdatedPositions, excludedKnownObjects); } - private boolean isNewObjectUpdateCloser(Translation2d newUpdate, Translation2d existingUpdate, Translation2d trackedObject) { + private boolean isNewObjectUpdateCloserThanPreviousUpdate(Translation2d newUpdate, Translation2d existingUpdate, Translation2d trackedObject) { return newUpdate.getDistance(trackedObject) < existingUpdate.getDistance(trackedObject); } - private boolean isObjectNew(Translation2d object, Translation2d nearestTrackedObject) { + private boolean isObjectAlreadyStored(Translation2d object, Translation2d nearestTrackedObject) { if (nearestTrackedObject == null) return true; return object.getDistance(nearestTrackedObject) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; @@ -224,10 +225,10 @@ private Translation2d getClosestObjectFromSetToPosition(Translation2d position, } private void removeOldObjects() { - objectPositionsToTimestamp.entrySet().removeIf(entry -> isTooOld(entry.getValue())); + objectPositionsToTimestamp.entrySet().removeIf(entry -> hasObjectExpired(entry.getValue())); } - private boolean isTooOld(double timestamp) { + private boolean hasObjectExpired(double timestamp) { return Timer.getTimestamp() - timestamp > deletionThresholdSeconds; } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java index cf97ae0f..528d1c7c 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java @@ -4,9 +4,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.lib.hardware.RobotHardwareStats; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; 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; /** @@ -39,14 +39,13 @@ public void periodic() { * @return the closest object's 2D position on the field (z is assumed to be 0) */ public Translation2d calculateClosestObjectPositionOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { - final Translation2d[] targetObjectsTranslation = getObjectsPositionsOnField(targetGamePiece); + final Translation2d[] targetObjectsTranslation = getObjectPositionsOnField(targetGamePiece); final Translation2d currentRobotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); if (targetObjectsTranslation.length == 0) return null; Translation2d closestObjectTranslation = targetObjectsTranslation[0]; - for (int i = 1; i < targetObjectsTranslation.length; i++) { - final Translation2d currentObjectTranslation = targetObjectsTranslation[i]; + for (Translation2d currentObjectTranslation : targetObjectsTranslation) { final double closestObjectDistanceToRobot = currentRobotTranslation.getDistance(closestObjectTranslation); final double currentObjectDistanceToRobot = currentRobotTranslation.getDistance(currentObjectTranslation); if (currentObjectDistanceToRobot < closestObjectDistanceToRobot) @@ -59,15 +58,15 @@ public boolean hasObject(SimulatedGamePieceConstants.GamePieceType targetGamePie return objectDetectionCameraInputs.hasObject[targetGamePiece.id]; } - public Translation2d[] getObjectsPositionsOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { - final Rotation3d[] visibleObjectsRotations = getObjectsRotations(targetGamePiece); - final Translation2d[] objectsPositionsOnField = new Translation2d[visibleObjectsRotations.length]; + public Translation2d[] getObjectPositionsOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + 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[] getObjectsRotations(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { @@ -75,7 +74,7 @@ public Rotation3d[] getObjectsRotations(SimulatedGamePieceConstants.GamePieceTyp } /** - * Calculates the position of the object on the field from its 3D rotation 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. diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCameraIO.java index 251bc164..6cf09732 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCameraIO.java @@ -14,13 +14,13 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { @AutoLog public static class ObjectDetectionCameraInputs { /** - * Whether there is at least one object or not for each game piece, by game piece index (type). + * Whether the camera sees at least one object or none of each game piece, by game piece index (type). */ public boolean[] hasObject = new boolean[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; /** * Stores the Rotation3d of all visible objects. * The first index is the game piece ID (type). - * The second index is the index of the game piece's Rotation3d, with the best object placed first (index 0). + * The second index is the index of the game piece's Rotation3d, with the closest object placed first (index 0). */ public Rotation3d[][] visibleObjectRotations = new Rotation3d[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; public double latestResultTimestamp = 0; From 9ff0e06b663a070329cfb024c2063cc45b8aa1f2 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 13 Jan 2026 17:41:40 +0200 Subject: [PATCH 61/68] moved double creation outside for loop --- .../misc/objectDetection/ObjectDetectionConstants.java | 2 +- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 4 ++-- .../objectdetectioncamera/ObjectDetectionCamera.java | 6 ++++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java index 242d9f83..6b11032b 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java @@ -4,5 +4,5 @@ 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; + static final double TRACKED_OBJECT_TOLERANCE_METERS = 0.12; } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index eb61efad..69e2edd5 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -152,7 +152,7 @@ private void updateObjectUpdates(HashMap currentTo private void updateObjectPosition(Translation2d object, HashMap trackedObjectsToUpdatedPositions) { final Translation2d nearestTrackedObject = getClosestTrackedObjectToPosition(object); - if (isObjectAlreadyStored(object, nearestTrackedObject)) + if (isObjectNotStored(object, nearestTrackedObject)) trackedObjectsToUpdatedPositions.put(object, object); else assignUpdateToTrackedObject(object, nearestTrackedObject, trackedObjectsToUpdatedPositions); @@ -192,7 +192,7 @@ private boolean isNewObjectUpdateCloserThanPreviousUpdate(Translation2d newUpdat existingUpdate.getDistance(trackedObject); } - private boolean isObjectAlreadyStored(Translation2d object, Translation2d nearestTrackedObject) { + private boolean isObjectNotStored(Translation2d object, Translation2d nearestTrackedObject) { if (nearestTrackedObject == null) return true; return object.getDistance(nearestTrackedObject) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java index 528d1c7c..262a1f0b 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java @@ -44,12 +44,14 @@ public Translation2d calculateClosestObjectPositionOnField(SimulatedGamePieceCon if (targetObjectsTranslation.length == 0) return null; Translation2d closestObjectTranslation = targetObjectsTranslation[0]; + double closestObjectDistanceToRobot = currentRobotTranslation.getDistance(closestObjectTranslation); for (Translation2d currentObjectTranslation : targetObjectsTranslation) { - final double closestObjectDistanceToRobot = currentRobotTranslation.getDistance(closestObjectTranslation); final double currentObjectDistanceToRobot = currentRobotTranslation.getDistance(currentObjectTranslation); - if (currentObjectDistanceToRobot < closestObjectDistanceToRobot) + if (currentObjectDistanceToRobot < closestObjectDistanceToRobot) { closestObjectTranslation = currentObjectTranslation; + closestObjectDistanceToRobot = currentRobotTranslation.getDistance(closestObjectTranslation); + } } return closestObjectTranslation; } From 018d4cf8946cf22a6d5a61e0ea0256c3b40b15d8 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 13 Jan 2026 20:48:50 +0200 Subject: [PATCH 62/68] simplified logic --- .../objectDetection/ObjectPoseEstimator.java | 76 ++++++++----------- 1 file changed, 31 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 69e2edd5..a5dbca2b 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -150,58 +150,44 @@ private void updateObjectUpdates(HashMap currentTo currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimestamp.put(object, currentTimestamp)); } - private void updateObjectPosition(Translation2d object, HashMap trackedObjectsToUpdatedPositions) { - final Translation2d nearestTrackedObject = getClosestTrackedObjectToPosition(object); - if (isObjectNotStored(object, nearestTrackedObject)) - trackedObjectsToUpdatedPositions.put(object, object); - else - assignUpdateToTrackedObject(object, nearestTrackedObject, trackedObjectsToUpdatedPositions); - } - - private void assignUpdateToTrackedObject(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap trackedObjectsToUpdatedPositions) { - if (trackedObjectsToUpdatedPositions.containsKey(objectToUpdate)) { - final Set excludedKnownObjects = new HashSet<>(); - addClosestUpdatedPositionToHashMap(objectsUpdatedPosition, objectToUpdate, trackedObjectsToUpdatedPositions, excludedKnownObjects); - } else - trackedObjectsToUpdatedPositions.put(objectToUpdate, objectsUpdatedPosition); - } - - private void addClosestUpdatedPositionToHashMap(Translation2d objectsUpdatedPosition, Translation2d objectToUpdate, HashMap trackedObjectsToUpdatedPositions, Set excludedKnownObjects) { - final Translation2d existingUpdatedPosition = trackedObjectsToUpdatedPositions.get(objectToUpdate); - if (isNewObjectUpdateCloserThanPreviousUpdate(objectsUpdatedPosition, existingUpdatedPosition, objectToUpdate)) { - trackedObjectsToUpdatedPositions.replace(objectToUpdate, objectsUpdatedPosition); - reassignDiscardedObjectUpdate(existingUpdatedPosition, objectToUpdate, trackedObjectsToUpdatedPositions, excludedKnownObjects); + private void updateObjectPosition(Translation2d objectUpdate, HashMap trackedObjectsToUpdatedPositions) { + final Translation2d closestAvailableTrackedObjectToVisibleObject = getClosestAvailableObjectToUpdate(objectUpdate, trackedObjectsToUpdatedPositions); + if (closestAvailableTrackedObjectToVisibleObject == null) { + trackedObjectsToUpdatedPositions.put(objectUpdate, objectUpdate); return; } - reassignDiscardedObjectUpdate(objectsUpdatedPosition, objectToUpdate, trackedObjectsToUpdatedPositions, excludedKnownObjects); + final Translation2d previousUpdate = trackedObjectsToUpdatedPositions.get(closestAvailableTrackedObjectToVisibleObject); + trackedObjectsToUpdatedPositions.replace(closestAvailableTrackedObjectToVisibleObject, objectUpdate); + updateObjectPosition(previousUpdate, trackedObjectsToUpdatedPositions); } - private void reassignDiscardedObjectUpdate(Translation2d discardedObjectUpdate, Translation2d previousClosestTrackedObject, HashMap trackedObjectsToUpdatedPositions, Set excludedKnownObjects) { - excludedKnownObjects.add(previousClosestTrackedObject); - final Translation2d nextClosestObjectToDiscardedObjectUpdate = getNextClosestKnownObjectToPosition(discardedObjectUpdate, excludedKnownObjects); - if (nextClosestObjectToDiscardedObjectUpdate == null || - discardedObjectUpdate.getDistance(nextClosestObjectToDiscardedObjectUpdate) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) { - trackedObjectsToUpdatedPositions.put(discardedObjectUpdate, discardedObjectUpdate); - return; + private Translation2d getClosestAvailableObjectToUpdate(Translation2d update, HashMap objectsWithUpdates) { + Set availableObjectsToUpdate = getAvailableObjectsToUpdate(update, objectsWithUpdates); + if (availableObjectsToUpdate.isEmpty()) + return null; + return getClosestObjectFromSetToPosition(update, availableObjectsToUpdate); + } + + private Set getAvailableObjectsToUpdate(Translation2d update, HashMap objectsWithUpdates) { + final Set availableObjects = new HashSet<>(); + for (Translation2d currentObject : objectPositionsToTimestamp.keySet()) { + final double updateDistanceFromCurrentObject = update.getDistance(currentObject); + if (updateDistanceFromCurrentObject > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) + continue; + if (!objectsWithUpdates.containsKey(currentObject)) { + availableObjects.add(currentObject); + continue; + } + if (update == getClosestUpdateToObject(update, objectsWithUpdates.get(currentObject), currentObject)) + availableObjects.add(currentObject); } - addClosestUpdatedPositionToHashMap(discardedObjectUpdate, nextClosestObjectToDiscardedObjectUpdate, trackedObjectsToUpdatedPositions, excludedKnownObjects); - } - - private boolean isNewObjectUpdateCloserThanPreviousUpdate(Translation2d newUpdate, Translation2d existingUpdate, Translation2d trackedObject) { - return newUpdate.getDistance(trackedObject) < - existingUpdate.getDistance(trackedObject); - } - - private boolean isObjectNotStored(Translation2d object, Translation2d nearestTrackedObject) { - if (nearestTrackedObject == null) - return true; - return object.getDistance(nearestTrackedObject) > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS; + return availableObjects; } - private Translation2d getNextClosestKnownObjectToPosition(Translation2d position, Set excludedKnownObjects) { - Set candidateObjects = new HashSet<>(objectPositionsToTimestamp.keySet()); - candidateObjects.removeAll(excludedKnownObjects); - return getClosestObjectFromSetToPosition(position, candidateObjects); + private Translation2d getClosestUpdateToObject(Translation2d newUpdate, Translation2d previousUpdate, Translation2d object) { + return newUpdate.getDistance(object) < previousUpdate.getDistance(object) + ? newUpdate + : previousUpdate; } private Translation2d getClosestTrackedObjectToPosition(Translation2d position) { From 64779515054b93c4d0cbf8f73d0b27f272ae7433 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 13 Jan 2026 20:49:23 +0200 Subject: [PATCH 63/68] Update ObjectPoseEstimator.java --- .../trigon/robot/misc/objectDetection/ObjectPoseEstimator.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index a5dbca2b..7302b74a 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -136,9 +136,8 @@ private void updateTrackedObjectsPositions() { final Translation2d[] visibleObjects = camera.getObjectPositionsOnField(gamePieceType); final HashMap trackedObjectsToUpdatedPositions = new HashMap<>(); - for (Translation2d visibleObject : visibleObjects) { + for (Translation2d visibleObject : visibleObjects) updateObjectPosition(visibleObject, trackedObjectsToUpdatedPositions); - } updateObjectUpdates(trackedObjectsToUpdatedPositions); } From fcda610e78ca1dec20c958fa48d8c0ad13ca2f47 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 13 Jan 2026 20:53:10 +0200 Subject: [PATCH 64/68] Update ObjectDetectionCamera.java --- .../objectdetectioncamera/ObjectDetectionCamera.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java index 262a1f0b..19b623a4 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java @@ -50,7 +50,7 @@ public Translation2d calculateClosestObjectPositionOnField(SimulatedGamePieceCon final double currentObjectDistanceToRobot = currentRobotTranslation.getDistance(currentObjectTranslation); if (currentObjectDistanceToRobot < closestObjectDistanceToRobot) { closestObjectTranslation = currentObjectTranslation; - closestObjectDistanceToRobot = currentRobotTranslation.getDistance(closestObjectTranslation); + closestObjectDistanceToRobot = currentObjectDistanceToRobot; } } return closestObjectTranslation; From 43c1a89a3bf371b7faa64a5c191b65e77cc087bc Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 13 Jan 2026 21:24:01 +0200 Subject: [PATCH 65/68] Update ObjectPoseEstimator.java --- .../robot/misc/objectDetection/ObjectPoseEstimator.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 7302b74a..7dbdd3ce 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -157,17 +157,20 @@ private void updateObjectPosition(Translation2d objectUpdate, HashMap objectsWithUpdates) { Set availableObjectsToUpdate = getAvailableObjectsToUpdate(update, objectsWithUpdates); - if (availableObjectsToUpdate.isEmpty()) + if (availableObjectsToUpdate == null || availableObjectsToUpdate.isEmpty()) return null; return getClosestObjectFromSetToPosition(update, availableObjectsToUpdate); } private Set getAvailableObjectsToUpdate(Translation2d update, HashMap objectsWithUpdates) { + if (objectPositionsToTimestamp.isEmpty()) + return null; final Set availableObjects = new HashSet<>(); for (Translation2d currentObject : objectPositionsToTimestamp.keySet()) { final double updateDistanceFromCurrentObject = update.getDistance(currentObject); From 5a799b20fbbc15946ffb0e211a414057ef1ebf34 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 13 Jan 2026 23:26:21 +0200 Subject: [PATCH 66/68] mergggge --- src/main/java/frc/trigon/robot/Robot.java | 2 + .../objectDetection/ObjectPoseEstimator.java | 38 +++++++++---------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/trigon/robot/Robot.java b/src/main/java/frc/trigon/robot/Robot.java index e938b926..147c1c91 100644 --- a/src/main/java/frc/trigon/robot/Robot.java +++ b/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 diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java index 7dbdd3ce..0be39ae7 100644 --- a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -22,7 +22,7 @@ public class ObjectPoseEstimator extends SubsystemBase { /** * Stores the position of each detected object along with the timestamp of when it was detected. */ - private final HashMap objectPositionsToTimestamp; + private final HashMap objectPositionsToDetectionTimestamp; /** * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. @@ -37,7 +37,7 @@ public ObjectPoseEstimator(double deletionThresholdSeconds, this.deletionThresholdSeconds = deletionThresholdSeconds; this.gamePieceType = gamePieceType; this.camera = camera; - this.objectPositionsToTimestamp = new HashMap<>(); + this.objectPositionsToDetectionTimestamp = new HashMap<>(); } /** @@ -57,7 +57,7 @@ public void periodic() { * @return a list of Translation2d representing the positions of objects on the field */ public ArrayList getObjectsOnField() { - return new ArrayList<>(objectPositionsToTimestamp.keySet()); + return new ArrayList<>(objectPositionsToDetectionTimestamp.keySet()); } /** @@ -111,7 +111,7 @@ public void removeClosestObjectToPosition(Translation2d 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) { - objectPositionsToTimestamp.remove(objectPosition); + objectPositionsToDetectionTimestamp.remove(objectPosition); } /** @@ -120,7 +120,7 @@ public void removeObject(Translation2d objectPosition) { * @return if there are objects stored in the pose estimator */ public boolean hasObjects() { - return !objectPositionsToTimestamp.isEmpty(); + return !objectPositionsToDetectionTimestamp.isEmpty(); } /** @@ -139,14 +139,14 @@ private void updateTrackedObjectsPositions() { for (Translation2d visibleObject : visibleObjects) updateObjectPosition(visibleObject, trackedObjectsToUpdatedPositions); - updateObjectUpdates(trackedObjectsToUpdatedPositions); + applyObjectUpdates(trackedObjectsToUpdatedPositions); } - private void updateObjectUpdates(HashMap currentToNewObjectPositions) { + private void applyObjectUpdates(HashMap currentToNewObjectPositions) { final double currentTimestamp = Timer.getTimestamp(); - objectPositionsToTimestamp.keySet().removeAll(currentToNewObjectPositions.keySet()); - currentToNewObjectPositions.values().forEach(object -> objectPositionsToTimestamp.put(object, currentTimestamp)); + objectPositionsToDetectionTimestamp.keySet().removeAll(currentToNewObjectPositions.keySet()); + currentToNewObjectPositions.values().forEach(object -> objectPositionsToDetectionTimestamp.put(object, currentTimestamp)); } private void updateObjectPosition(Translation2d objectUpdate, HashMap trackedObjectsToUpdatedPositions) { @@ -156,23 +156,23 @@ private void updateObjectPosition(Translation2d objectUpdate, HashMap objectsWithUpdates) { - Set availableObjectsToUpdate = getAvailableObjectsToUpdate(update, objectsWithUpdates); + final Set availableObjectsToUpdate = getAvailableObjectsToUpdate(update, objectsWithUpdates); if (availableObjectsToUpdate == null || availableObjectsToUpdate.isEmpty()) return null; return getClosestObjectFromSetToPosition(update, availableObjectsToUpdate); } private Set getAvailableObjectsToUpdate(Translation2d update, HashMap objectsWithUpdates) { - if (objectPositionsToTimestamp.isEmpty()) + if (objectPositionsToDetectionTimestamp.isEmpty()) return null; final Set availableObjects = new HashSet<>(); - for (Translation2d currentObject : objectPositionsToTimestamp.keySet()) { + for (Translation2d currentObject : objectPositionsToDetectionTimestamp.keySet()) { final double updateDistanceFromCurrentObject = update.getDistance(currentObject); if (updateDistanceFromCurrentObject > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) continue; @@ -180,20 +180,18 @@ private Set getAvailableObjectsToUpdate(Translation2d update, Has availableObjects.add(currentObject); continue; } - if (update == getClosestUpdateToObject(update, objectsWithUpdates.get(currentObject), currentObject)) + if (isNewUpdateCloserThanPreviousUpdate(update, objectsWithUpdates.get(currentObject), currentObject)) availableObjects.add(currentObject); } return availableObjects; } - private Translation2d getClosestUpdateToObject(Translation2d newUpdate, Translation2d previousUpdate, Translation2d object) { - return newUpdate.getDistance(object) < previousUpdate.getDistance(object) - ? newUpdate - : previousUpdate; + private boolean isNewUpdateCloserThanPreviousUpdate(Translation2d newUpdate, Translation2d previousUpdate, Translation2d object) { + return newUpdate.getDistance(object) < previousUpdate.getDistance(object); } private Translation2d getClosestTrackedObjectToPosition(Translation2d position) { - return getClosestObjectFromSetToPosition(position, objectPositionsToTimestamp.keySet()); + return getClosestObjectFromSetToPosition(position, objectPositionsToDetectionTimestamp.keySet()); } private Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set objects) { @@ -213,7 +211,7 @@ private Translation2d getClosestObjectFromSetToPosition(Translation2d position, } private void removeOldObjects() { - objectPositionsToTimestamp.entrySet().removeIf(entry -> hasObjectExpired(entry.getValue())); + objectPositionsToDetectionTimestamp.entrySet().removeIf(entry -> hasObjectExpired(entry.getValue())); } private boolean hasObjectExpired(double timestamp) { From 342eb1903b06839e5f32e1e456a9dafb4fa14e60 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 13 Jan 2026 23:33:50 +0200 Subject: [PATCH 67/68] sdfgh --- src/main/java/frc/trigon/lib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 77c1f252..54491b3e 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 77c1f2526dfb1dc1d53842927a70e48ead1cf20b +Subproject commit 54491b3e0a6e771642f7185bc3b579c991a1114a From 330da69cbe4cd0e1eb761ad34bff51d222636e28 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 13 Jan 2026 23:37:17 +0200 Subject: [PATCH 68/68] Update lib --- src/main/java/frc/trigon/lib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 54491b3e..9403200d 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 54491b3e0a6e771642f7185bc3b579c991a1114a +Subproject commit 9403200d5d1161a2c75be560251618af01230b5a