From b124ff15a26dafbd5181281e8d730dc4992459d1 Mon Sep 17 00:00:00 2001 From: Steggo 4467 <114831754+Steggoo@users.noreply.github.com> Date: Thu, 25 Jan 2024 19:56:18 -0500 Subject: [PATCH 01/13] Feat: Added PhotonLib --- build.gradle | 2 +- vendordeps/photonlib.json | 57 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 58 insertions(+), 1 deletion(-) create mode 100644 vendordeps/photonlib.json diff --git a/build.gradle b/build.gradle index 1114ba98..f6c7e7d7 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.1.1" + id "edu.wpi.first.GradleRIO" version "2024.2.1" id "com.peterabeles.gversion" version "1.10" id "org.sonarqube" version "4.4.1.3373" } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 00000000..294c3398 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.2.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2024.2.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.2.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2024.2.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.2.1" + } + ] +} From cefe6cc0bd3cce3a39b9333d01dcf6decb6e774a Mon Sep 17 00:00:00 2001 From: Steggo 4467 <114831754+Steggoo@users.noreply.github.com> Date: Thu, 25 Jan 2024 20:01:57 -0500 Subject: [PATCH 02/13] Feat: Added Subsystem --- .../robot/subsystems/vision/VisionSubsystem.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java new file mode 100644 index 00000000..89cea290 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.vision; + + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class VisionSubsystem extends SubsystemBase { + public VisionSubsystem() { + // TODO: Set the default command, if any, for this subsystem by calling setDefaultCommand(command) + // in the constructor or in the robot coordination class, such as RobotContainer. + // Also, you can call addChild(name, sendableChild) to associate sendables with the subsystem + // such as SpeedControllers, Encoders, DigitalInputs, etc. + } +} + From 65b97adc4cf3ce51bead53dffd79ec1d1003df71 Mon Sep 17 00:00:00 2001 From: ItchyMoose281 Date: Thu, 25 Jan 2024 20:40:18 -0500 Subject: [PATCH 03/13] Update VisionSubsystem.java Feat: Created constructor for PhotonVision Subsystem --- .../subsystems/vision/VisionSubsystem.java | 42 ++++++++++++++++--- 1 file changed, 37 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 89cea290..7f4bd73a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -1,14 +1,46 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; + +import java.io.IOException; public class VisionSubsystem extends SubsystemBase { - public VisionSubsystem() { - // TODO: Set the default command, if any, for this subsystem by calling setDefaultCommand(command) - // in the constructor or in the robot coordination class, such as RobotContainer. - // Also, you can call addChild(name, sendableChild) to associate sendables with the subsystem - // such as SpeedControllers, Encoders, DigitalInputs, etc. + private final PhotonCamera m_camera; + private final Transform3d robotToCam; + private PhotonPoseEstimator m_photonPoseEstimator; + private final DriverStation.Alliance m_alliance; + private AprilTagFieldLayout m_aprilTagFieldLayout; + public VisionSubsystem(String camName, Transform3d camPose) { + m_camera = new PhotonCamera(camName); + robotToCam = camPose; + if(DriverStation.getAlliance()!=null) { + m_alliance = DriverStation.getAlliance().get(); + } else { + m_alliance = DriverStation.Alliance.Blue; + } + try { + m_aprilTagFieldLayout =AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); + if(m_alliance == DriverStation.Alliance.Blue){ + m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); + } else { + m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); + } + m_photonPoseEstimator = new PhotonPoseEstimator( + m_aprilTagFieldLayout, + PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, + m_camera, + robotToCam); + } catch (IOException e){ + throw new IllegalStateException(e); + } + } } From 0baa41712929154ec0d99190d2db23c2b5f5b285 Mon Sep 17 00:00:00 2001 From: ItchyMoose281 Date: Thu, 25 Jan 2024 21:56:50 -0500 Subject: [PATCH 04/13] Feat: Added getPose method to vision subsystem. --- .../subsystems/vision/VisionSubsystem.java | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 7f4bd73a..808e6b70 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -3,19 +3,23 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; +import org.photonvision.targeting.PhotonPipelineResult; import java.io.IOException; +import java.util.Optional; public class VisionSubsystem extends SubsystemBase { private final PhotonCamera m_camera; private final Transform3d robotToCam; private PhotonPoseEstimator m_photonPoseEstimator; - private final DriverStation.Alliance m_alliance; + private DriverStation.Alliance m_alliance; private AprilTagFieldLayout m_aprilTagFieldLayout; public VisionSubsystem(String camName, Transform3d camPose) { m_camera = new PhotonCamera(camName); @@ -40,7 +44,18 @@ public VisionSubsystem(String camName, Transform3d camPose) { } catch (IOException e){ throw new IllegalStateException(e); } - + } + public Optional getPose(Pose2d prevEstimatedRobotPose){ + m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); + if(m_alliance != DriverStation.getAlliance().get()); + m_alliance = DriverStation.getAlliance().get(); + if(m_alliance == DriverStation.Alliance.Blue){ + m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); + } else { + m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); + } + PhotonPipelineResult camResult = m_camera.getLatestResult(); + return m_photonPoseEstimator.update(camResult); } } From 486fc023d9c12ab95a15cec58ab0741c762075f0 Mon Sep 17 00:00:00 2001 From: Steggo 4467 <114831754+Steggoo@users.noreply.github.com> Date: Thu, 25 Jan 2024 21:59:19 -0500 Subject: [PATCH 05/13] Feat: Started to create auto logger in VisionSubsystem --- .../frc/robot/subsystems/vision/VisionSubsystem.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 808e6b70..a91effcc 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -21,6 +21,15 @@ public class VisionSubsystem extends SubsystemBase { private PhotonPoseEstimator m_photonPoseEstimator; private DriverStation.Alliance m_alliance; private AprilTagFieldLayout m_aprilTagFieldLayout; + + public class photonVisionInputs { + public boolean camConnected = false; + public double camLatency = 0.0; + public boolean tagsFound = false; + public int[] tagIds = new int[] {}; + public int nearestTagId = 0; + public Pose2d nearestTagPose = new Pose2d(); + } public VisionSubsystem(String camName, Transform3d camPose) { m_camera = new PhotonCamera(camName); robotToCam = camPose; From 68edbd1ec689d45808bfc5ffe9043b2e477a861b Mon Sep 17 00:00:00 2001 From: Steggo 4467 <114831754+Steggoo@users.noreply.github.com> Date: Sat, 3 Feb 2024 11:29:20 -0500 Subject: [PATCH 06/13] Feat: Added Update Inputs (Incomplete) --- .../subsystems/vision/VisionSubsystem.java | 156 +++++++++++++----- vendordeps/AdvantageKit.json | 12 +- 2 files changed, 118 insertions(+), 50 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index a91effcc..7241ace6 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.AutoLog; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -16,55 +17,122 @@ import java.util.Optional; public class VisionSubsystem extends SubsystemBase { - private final PhotonCamera m_camera; - private final Transform3d robotToCam; - private PhotonPoseEstimator m_photonPoseEstimator; - private DriverStation.Alliance m_alliance; - private AprilTagFieldLayout m_aprilTagFieldLayout; - - public class photonVisionInputs { - public boolean camConnected = false; - public double camLatency = 0.0; - public boolean tagsFound = false; - public int[] tagIds = new int[] {}; - public int nearestTagId = 0; - public Pose2d nearestTagPose = new Pose2d(); + + @AutoLog + public class PhotonVisionIO { + protected boolean camConnected = false; + protected double camLatency = 0.0; + protected boolean tagsFound = false; + protected int[] tagIds = new int[] {}; + protected int numTagsFound = 0; + + public boolean isCamConnected() { + return camConnected; } - public VisionSubsystem(String camName, Transform3d camPose) { - m_camera = new PhotonCamera(camName); - robotToCam = camPose; - if(DriverStation.getAlliance()!=null) { - m_alliance = DriverStation.getAlliance().get(); - } else { - m_alliance = DriverStation.Alliance.Blue; - } - try { - m_aprilTagFieldLayout =AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); - if(m_alliance == DriverStation.Alliance.Blue){ - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); - } else { - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); - } - m_photonPoseEstimator = new PhotonPoseEstimator( - m_aprilTagFieldLayout, - PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, - m_camera, - robotToCam); - } catch (IOException e){ - throw new IllegalStateException(e); - } + + public void setCamConnected(boolean camConnected) { + this.camConnected = camConnected; + } + + public double getCamLatency() { + return camLatency; + } + + public void setCamLatency(double camLatency) { + this.camLatency = camLatency; + } + + public boolean isTagsFound() { + return tagsFound; + } + + public void setTagsFound(boolean tagsFound) { + this.tagsFound = tagsFound; + } + + public int[] getTagIds() { + return tagIds; + } + + public void setTagIds(int[] tagIds) { + this.tagIds = tagIds; } - public Optional getPose(Pose2d prevEstimatedRobotPose){ - m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); - if(m_alliance != DriverStation.getAlliance().get()); + + public int getNumTagsFound() { + return numTagsFound; + } + + public void setNumTagsFound(int numTagsFound) { + this.numTagsFound = numTagsFound; + } + } + + private final PhotonCamera m_camera; + private final Transform3d robotToCam; + private PhotonPoseEstimator m_photonPoseEstimator; + private DriverStation.Alliance m_alliance; + private AprilTagFieldLayout m_aprilTagFieldLayout; + + private final PhotonVisionIO inputs = new PhotonVisionIO(); + + private void updateInputs(){ + inputs.setCamConnected(m_camera.isConnected()); + inputs.setCamLatency(m_camera.getLatestResult().getLatencyMillis()); +// inputs.setTagIds(m_camera.getLatestResult().getTargets().); + } + + public VisionSubsystem(String camName, Transform3d camPose) { + m_camera = new PhotonCamera(camName); + robotToCam = camPose; + if(DriverStation.getAlliance().isPresent()) { + m_alliance = DriverStation.getAlliance().get(); + } else { + m_alliance = DriverStation.Alliance.Blue; + } + try { + m_aprilTagFieldLayout =AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); + if(m_alliance == DriverStation.Alliance.Blue){ + m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); + } else { + m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); + } + m_photonPoseEstimator = new PhotonPoseEstimator( + m_aprilTagFieldLayout, + PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, + m_camera, + robotToCam); + } catch (IOException e){ + throw new IllegalStateException(e); + } + } + + @Override + public void periodic() { + updateInputs(); + super.periodic(); + } + + public Optional getPose(Pose2d prevEstimatedRobotPose) { + + m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); + + if(DriverStation.getAlliance().isPresent()){ + if (m_alliance != DriverStation.getAlliance().get()){ m_alliance = DriverStation.getAlliance().get(); - if(m_alliance == DriverStation.Alliance.Blue){ - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); + + if (m_alliance == DriverStation.Alliance.Red) { + m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); } else { - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); + m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); } - PhotonPipelineResult camResult = m_camera.getLatestResult(); - return m_photonPoseEstimator.update(camResult); + } } + else { + m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); + } + + PhotonPipelineResult camResult = m_camera.getLatestResult(); + return m_photonPoseEstimator.update(camResult); + } } diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index a3a16b69..d0cc0d12 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.0.0", + "version": "3.0.2", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.0.0" + "version": "3.0.2" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.0.0" + "version": "3.0.2" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.0.0" + "version": "3.0.2" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.0.0", + "version": "3.0.2", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ @@ -39,4 +39,4 @@ } ], "cppDependencies": [] -} +} \ No newline at end of file From 5c38444564b1226b174a4b1f5ef2702d188ebc18 Mon Sep 17 00:00:00 2001 From: Steggo 4467 <114831754+Steggoo@users.noreply.github.com> Date: Sat, 3 Feb 2024 14:38:24 -0500 Subject: [PATCH 07/13] Feat: Finished UpdateInputs method in VisionSubsystem --- .../subsystems/vision/VisionSubsystem.java | 41 ++++++++++++++----- 1 file changed, 31 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 7241ace6..102bcd1d 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -42,14 +42,6 @@ public void setCamLatency(double camLatency) { this.camLatency = camLatency; } - public boolean isTagsFound() { - return tagsFound; - } - - public void setTagsFound(boolean tagsFound) { - this.tagsFound = tagsFound; - } - public int[] getTagIds() { return tagIds; } @@ -65,6 +57,14 @@ public int getNumTagsFound() { public void setNumTagsFound(int numTagsFound) { this.numTagsFound = numTagsFound; } + + public boolean isTagsFound() { + return tagsFound; + } + + public void setTagsFound(boolean tagsFound) { + this.tagsFound = tagsFound; + } } private final PhotonCamera m_camera; @@ -75,10 +75,12 @@ public void setNumTagsFound(int numTagsFound) { private final PhotonVisionIO inputs = new PhotonVisionIO(); - private void updateInputs(){ + public void updateInputs(){ inputs.setCamConnected(m_camera.isConnected()); inputs.setCamLatency(m_camera.getLatestResult().getLatencyMillis()); -// inputs.setTagIds(m_camera.getLatestResult().getTargets().); + inputs.setTagIds(getTargetIDs()); + inputs.setNumTagsFound(m_camera.getLatestResult().targets.size()); + inputs.setTagsFound(m_camera.getLatestResult().hasTargets()); } public VisionSubsystem(String camName, Transform3d camPose) { @@ -134,5 +136,24 @@ public Optional getPose(Pose2d prevEstimatedRobotPose) { PhotonPipelineResult camResult = m_camera.getLatestResult(); return m_photonPoseEstimator.update(camResult); } + + public int[] getTargetIDs () { + PhotonPipelineResult targets = m_camera.getLatestResult(); + int[] results; + + if(targets.hasTargets()) { + results = new int[targets.getTargets().size()]; + + for (int i = 0; i < results.length; i++) { + results[i] = targets.getTargets().get(i).getFiducialId(); + } + + } + else { + results = new int[0]; + } + + return results; + } } From e23128d494e03c310e73d3300c4ea8dd55970f26 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 8 Feb 2024 10:26:15 -0500 Subject: [PATCH 08/13] commented out archaic bug fix --- .../vision/PhotonVisionIOInputs.java | 52 +++++++++ .../subsystems/vision/VisionSubsystem.java | 100 +++++------------- 2 files changed, 78 insertions(+), 74 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java new file mode 100644 index 00000000..e859e47f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java @@ -0,0 +1,52 @@ +package frc.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; + +@AutoLog +public class PhotonVisionIOInputs { + protected boolean camConnected = false; + protected double camLatency = 0.0; + protected boolean tagsFound = false; + protected int[] tagIds = new int[] {}; + protected int numTagsFound = 0; + + public boolean isCamConnected() { + return camConnected; + } + + public void setCamConnected(boolean camConnected) { + this.camConnected = camConnected; + } + + public double getCamLatency() { + return camLatency; + } + + public void setCamLatency(double camLatency) { + this.camLatency = camLatency; + } + + public int[] getTagIds() { + return tagIds; + } + + public void setTagIds(int[] tagIds) { + this.tagIds = tagIds; + } + + public int getNumTagsFound() { + return numTagsFound; + } + + public void setNumTagsFound(int numTagsFound) { + this.numTagsFound = numTagsFound; + } + + public boolean isTagsFound() { + return tagsFound; + } + + public void setTagsFound(boolean tagsFound) { + this.tagsFound = tagsFound; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 102bcd1d..3de3677a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -18,62 +18,13 @@ public class VisionSubsystem extends SubsystemBase { - @AutoLog - public class PhotonVisionIO { - protected boolean camConnected = false; - protected double camLatency = 0.0; - protected boolean tagsFound = false; - protected int[] tagIds = new int[] {}; - protected int numTagsFound = 0; - - public boolean isCamConnected() { - return camConnected; - } - - public void setCamConnected(boolean camConnected) { - this.camConnected = camConnected; - } - - public double getCamLatency() { - return camLatency; - } - - public void setCamLatency(double camLatency) { - this.camLatency = camLatency; - } - - public int[] getTagIds() { - return tagIds; - } - - public void setTagIds(int[] tagIds) { - this.tagIds = tagIds; - } - - public int getNumTagsFound() { - return numTagsFound; - } - - public void setNumTagsFound(int numTagsFound) { - this.numTagsFound = numTagsFound; - } - - public boolean isTagsFound() { - return tagsFound; - } - - public void setTagsFound(boolean tagsFound) { - this.tagsFound = tagsFound; - } - } - private final PhotonCamera m_camera; private final Transform3d robotToCam; private PhotonPoseEstimator m_photonPoseEstimator; private DriverStation.Alliance m_alliance; private AprilTagFieldLayout m_aprilTagFieldLayout; - private final PhotonVisionIO inputs = new PhotonVisionIO(); + private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged(); public void updateInputs(){ inputs.setCamConnected(m_camera.isConnected()); @@ -86,18 +37,18 @@ public void updateInputs(){ public VisionSubsystem(String camName, Transform3d camPose) { m_camera = new PhotonCamera(camName); robotToCam = camPose; - if(DriverStation.getAlliance().isPresent()) { - m_alliance = DriverStation.getAlliance().get(); - } else { - m_alliance = DriverStation.Alliance.Blue; - } +// if(DriverStation.getAlliance().isPresent()) { +// m_alliance = DriverStation.getAlliance().get(); +// } else { +// m_alliance = DriverStation.Alliance.Blue; +// } try { m_aprilTagFieldLayout =AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); - if(m_alliance == DriverStation.Alliance.Blue){ - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); - } else { - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); - } +// if(m_alliance == DriverStation.Alliance.Blue){ +// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); +// } else { +// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); +// } m_photonPoseEstimator = new PhotonPoseEstimator( m_aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, @@ -118,20 +69,21 @@ public Optional getPose(Pose2d prevEstimatedRobotPose) { m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); - if(DriverStation.getAlliance().isPresent()){ - if (m_alliance != DriverStation.getAlliance().get()){ - m_alliance = DriverStation.getAlliance().get(); - - if (m_alliance == DriverStation.Alliance.Red) { - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); - } else { - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); - } - } - } - else { - m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); - } + // We shouldn't have to do this anymore, as pathplanners new lib doesn't have the weird origin thing +// if(DriverStation.getAlliance().isPresent()){ +// if (m_alliance != DriverStation.getAlliance().get()){ +// m_alliance = DriverStation.getAlliance().get(); +// +// if (m_alliance == DriverStation.Alliance.Red) { +// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); +// } else { +// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); +// } +// } +// } +// else { +// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); +// } PhotonPipelineResult camResult = m_camera.getLatestResult(); return m_photonPoseEstimator.update(camResult); From 1fafbc2f7a6a2809c131940338598bd6fbd064f3 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 8 Feb 2024 18:00:21 -0500 Subject: [PATCH 09/13] finished implementing camera setup for testbed --- .../subsystems/drive/DriveSubsystem.java | 40 ++++++++++- .../subsystems/vision/VisionSubsystem.java | 68 ++++++++++++++++--- 2 files changed, 98 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 118065c9..5c2489f0 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -18,6 +18,8 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -26,12 +28,15 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.drive.module.Module; import frc.robot.subsystems.drive.module.ModuleIO; +import frc.robot.subsystems.vision.VisionSubsystem; import frc.robot.util.LocalADStarAK; import java.util.List; @@ -55,23 +60,45 @@ public class DriveSubsystem extends SubsystemBase { private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Pose2d pose = new Pose2d(); + private Pose2d visionPose = new Pose2d(); private Rotation2d lastGyroRotation = new Rotation2d(); + private final VisionSubsystem[] m_cameras; + private final Matrix m_stdDevs; + public DriveSubsystem( GyroIO gyroIO, ModuleIO flModuleIO, ModuleIO frModuleIO, ModuleIO blModuleIO, ModuleIO brModuleIO) { + this (gyroIO, + flModuleIO, + frModuleIO, + blModuleIO, + brModuleIO, + new VisionSubsystem[]{}); + } + + public DriveSubsystem( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO, + VisionSubsystem[] cameras) { this.gyroIO = gyroIO; modules[0] = new Module(flModuleIO); modules[1] = new Module(frModuleIO); modules[2] = new Module(blModuleIO); modules[3] = new Module(brModuleIO); + m_cameras = cameras; + m_stdDevs = VecBuilder.fill(Units.inchesToMeters(0.5), Units.inchesToMeters(0.5), Units.degreesToRadians(15)); + // Configure AutoBuilder for PathPlanner AutoBuilder.configureHolonomic( - this::getPose, + this::getVisionPose, this::setPose, () -> kinematics.toChassisSpeeds(getModuleStates()), this::runVelocity, @@ -143,6 +170,11 @@ public void periodic() { // Apply the twist (change since last sample) to the current pose pose = pose.exp(twist); } + + for (VisionSubsystem camera : m_cameras) { + var cameraPose = camera.getPose(getPose()); + cameraPose.ifPresent(visionUpdate -> visionPose = visionUpdate.apply(pose, m_stdDevs)); + } } /** @@ -218,6 +250,12 @@ public Pose2d getPose() { return pose; } + /** Returns the robot pose with vision updates */ + @AutoLogOutput(key = "Odometry/RobotVision") + public Pose2d getVisionPose() { + return visionPose; + } + /** Returns the current odometry rotation. */ public Rotation2d getRotation() { return pose.getRotation(); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 3de3677a..e99d74fe 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -3,11 +3,19 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -16,13 +24,14 @@ import java.io.IOException; import java.util.Optional; -public class VisionSubsystem extends SubsystemBase { +public class VisionSubsystem { private final PhotonCamera m_camera; private final Transform3d robotToCam; private PhotonPoseEstimator m_photonPoseEstimator; private DriverStation.Alliance m_alliance; private AprilTagFieldLayout m_aprilTagFieldLayout; + private final String m_name; private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged(); @@ -32,10 +41,13 @@ public void updateInputs(){ inputs.setTagIds(getTargetIDs()); inputs.setNumTagsFound(m_camera.getLatestResult().targets.size()); inputs.setTagsFound(m_camera.getLatestResult().hasTargets()); + + Logger.processInputs("PhotonVision/" + m_name, inputs); } public VisionSubsystem(String camName, Transform3d camPose) { m_camera = new PhotonCamera(camName); + m_name = camName; robotToCam = camPose; // if(DriverStation.getAlliance().isPresent()) { // m_alliance = DriverStation.getAlliance().get(); @@ -59,13 +71,7 @@ public VisionSubsystem(String camName, Transform3d camPose) { } } - @Override - public void periodic() { - updateInputs(); - super.periodic(); - } - - public Optional getPose(Pose2d prevEstimatedRobotPose) { + public Optional getPose(Pose2d prevEstimatedRobotPose) { m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); @@ -86,7 +92,16 @@ public Optional getPose(Pose2d prevEstimatedRobotPose) { // } PhotonPipelineResult camResult = m_camera.getLatestResult(); - return m_photonPoseEstimator.update(camResult); + Optional opPose = m_photonPoseEstimator.update(camResult); + + if (opPose.isEmpty()) { + return Optional.empty(); + } else { + EstimatedRobotPose estPose = opPose.get(); + return Optional.of(new VisionUpdate(estPose.estimatedPose.toPose2d(), + VecBuilder.fill(Units.inchesToMeters(0.5), Units.inchesToMeters(0.5), Units.degreesToRadians(15)), + estPose.timestampSeconds)); + } } public int[] getTargetIDs () { @@ -107,5 +122,40 @@ public int[] getTargetIDs () { return results; } + + public record VisionUpdate(Pose2d pose, Matrix stdDevs, double timestamp) { + public Pose2d apply(Pose2d lastPose, Matrix q) { + // Apply vision updates + // Calculate Kalman gains based on std devs + // (https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/estimator/) + Matrix visionK = new Matrix<>(Nat.N3(), Nat.N3()); + var r = new double[3]; + for (int i = 0; i < 3; ++i) { + r[i] = this.stdDevs().get(i, 0) * this.stdDevs().get(i, 0); + } + for (int row = 0; row < 3; ++row) { + if (q.get(row, 0) == 0.0) { + visionK.set(row, row, 0.0); + } else { + visionK.set( + row, row, q.get(row, 0) / (q.get(row, 0) + Math.sqrt(q.get(row, 0) * r[row]))); + } + } + + // Calculate twist between current and vision pose + var visionTwist = lastPose.log(this.pose()); + + // Multiply by Kalman gain matrix + var twistMatrix = + visionK.times(VecBuilder.fill(visionTwist.dx, visionTwist.dy, visionTwist.dtheta)); + + // Apply twist + lastPose = + lastPose.exp( + new Twist2d(twistMatrix.get(0, 0), twistMatrix.get(1, 0), twistMatrix.get(2, 0))); + + return lastPose; + } + } } From 094f254e815a693cae73450fec5dd3f3a8bb6ba0 Mon Sep 17 00:00:00 2001 From: Steggo 4467 <114831754+Steggoo@users.noreply.github.com> Date: Thu, 8 Feb 2024 20:35:27 -0500 Subject: [PATCH 10/13] Fix: Removed commented out logic --- .../subsystems/vision/VisionSubsystem.java | 26 ------------------- 1 file changed, 26 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index e99d74fe..70bc1590 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -49,18 +49,8 @@ public VisionSubsystem(String camName, Transform3d camPose) { m_camera = new PhotonCamera(camName); m_name = camName; robotToCam = camPose; -// if(DriverStation.getAlliance().isPresent()) { -// m_alliance = DriverStation.getAlliance().get(); -// } else { -// m_alliance = DriverStation.Alliance.Blue; -// } try { m_aprilTagFieldLayout =AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); -// if(m_alliance == DriverStation.Alliance.Blue){ -// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); -// } else { -// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); -// } m_photonPoseEstimator = new PhotonPoseEstimator( m_aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE, @@ -75,22 +65,6 @@ public Optional getPose(Pose2d prevEstimatedRobotPose) { m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); - // We shouldn't have to do this anymore, as pathplanners new lib doesn't have the weird origin thing -// if(DriverStation.getAlliance().isPresent()){ -// if (m_alliance != DriverStation.getAlliance().get()){ -// m_alliance = DriverStation.getAlliance().get(); -// -// if (m_alliance == DriverStation.Alliance.Red) { -// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); -// } else { -// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); -// } -// } -// } -// else { -// m_aprilTagFieldLayout.setOrigin(AprilTagFieldLayout.OriginPosition.kBlueAllianceWallRightSide); -// } - PhotonPipelineResult camResult = m_camera.getLatestResult(); Optional opPose = m_photonPoseEstimator.update(camResult); From 059c7c3c524a9bf5307264f4374e1468aef20669 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 8 Feb 2024 22:16:36 -0500 Subject: [PATCH 11/13] refactored pose estimation to properly account for timestamps --- .../subsystems/drive/DriveSubsystem.java | 42 ++--- .../subsystems/vision/VisionSubsystem.java | 9 +- .../java/frc/robot/util/PoseEstimator.java | 176 ++++++++++++++++++ src/main/java/lib/GeomUtil.java | 144 ++++++++++++++ 4 files changed, 345 insertions(+), 26 deletions(-) create mode 100644 src/main/java/frc/robot/util/PoseEstimator.java create mode 100644 src/main/java/lib/GeomUtil.java diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 5c2489f0..c28054ba 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -33,15 +33,22 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.drive.module.Module; import frc.robot.subsystems.drive.module.ModuleIO; import frc.robot.subsystems.vision.VisionSubsystem; import frc.robot.util.LocalADStarAK; +import java.util.Arrays; import java.util.List; +import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import java.util.stream.Collectors; +import java.util.stream.Stream; + +import frc.robot.util.PoseEstimator; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -59,26 +66,12 @@ public class DriveSubsystem extends SubsystemBase { private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); + private final PoseEstimator m_poseEstimator; private Pose2d pose = new Pose2d(); private Pose2d visionPose = new Pose2d(); private Rotation2d lastGyroRotation = new Rotation2d(); private final VisionSubsystem[] m_cameras; - private final Matrix m_stdDevs; - - public DriveSubsystem( - GyroIO gyroIO, - ModuleIO flModuleIO, - ModuleIO frModuleIO, - ModuleIO blModuleIO, - ModuleIO brModuleIO) { - this (gyroIO, - flModuleIO, - frModuleIO, - blModuleIO, - brModuleIO, - new VisionSubsystem[]{}); - } public DriveSubsystem( GyroIO gyroIO, @@ -94,7 +87,10 @@ public DriveSubsystem( modules[3] = new Module(brModuleIO); m_cameras = cameras; - m_stdDevs = VecBuilder.fill(Units.inchesToMeters(0.5), Units.inchesToMeters(0.5), Units.degreesToRadians(15)); + m_poseEstimator = + new PoseEstimator(VecBuilder.fill(Units.inchesToMeters(0.5), + Units.inchesToMeters(0.5), + Units.degreesToRadians(15))); // Configure AutoBuilder for PathPlanner AutoBuilder.configureHolonomic( @@ -168,13 +164,15 @@ public void periodic() { } Logger.recordOutput("Odometry/Twist", twist); // Apply the twist (change since last sample) to the current pose + m_poseEstimator.addDriveData(Timer.getFPGATimestamp(), twist); pose = pose.exp(twist); } - - for (VisionSubsystem camera : m_cameras) { - var cameraPose = camera.getPose(getPose()); - cameraPose.ifPresent(visionUpdate -> visionPose = visionUpdate.apply(pose, m_stdDevs)); - } + m_poseEstimator.addVisionData( + Arrays.stream(m_cameras) + .flatMap((VisionSubsystem camera) -> Stream.of(camera.getPose(m_poseEstimator.getLatestPose()))) + .filter(Optional::isPresent) + .flatMap(update -> Stream.of(update.get())) + .collect(Collectors.toList())); } /** @@ -253,7 +251,7 @@ public Pose2d getPose() { /** Returns the robot pose with vision updates */ @AutoLogOutput(key = "Odometry/RobotVision") public Pose2d getVisionPose() { - return visionPose; + return m_poseEstimator.getLatestPose(); } /** Returns the current odometry rotation. */ diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 70bc1590..1fa4d1e1 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.PoseEstimator; import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; @@ -61,7 +62,7 @@ public VisionSubsystem(String camName, Transform3d camPose) { } } - public Optional getPose(Pose2d prevEstimatedRobotPose) { + public Optional getPose(Pose2d prevEstimatedRobotPose) { m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); @@ -72,9 +73,9 @@ public Optional getPose(Pose2d prevEstimatedRobotPose) { return Optional.empty(); } else { EstimatedRobotPose estPose = opPose.get(); - return Optional.of(new VisionUpdate(estPose.estimatedPose.toPose2d(), - VecBuilder.fill(Units.inchesToMeters(0.5), Units.inchesToMeters(0.5), Units.degreesToRadians(15)), - estPose.timestampSeconds)); + return Optional.of(new PoseEstimator.TimestampedVisionUpdate(estPose.timestampSeconds, + estPose.estimatedPose.toPose2d(), + VecBuilder.fill(Units.inchesToMeters(0.5), Units.inchesToMeters(0.5), Units.degreesToRadians(15)))); } } diff --git a/src/main/java/frc/robot/util/PoseEstimator.java b/src/main/java/frc/robot/util/PoseEstimator.java new file mode 100644 index 00000000..ec3de406 --- /dev/null +++ b/src/main/java/frc/robot/util/PoseEstimator.java @@ -0,0 +1,176 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Timer; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; +import java.util.NavigableMap; +import java.util.TreeMap; +import lib.GeomUtil; + +public class PoseEstimator { + private static final double historyLengthSecs = 0.3; + + private Pose2d basePose = new Pose2d(); + private Pose2d latestPose = new Pose2d(); + private final NavigableMap updates = new TreeMap<>(); + private final Matrix q = new Matrix<>(Nat.N3(), Nat.N1()); + + public PoseEstimator(Matrix stateStdDevs) { + for (int i = 0; i < 3; ++i) { + q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); + } + } + + /** Returns the latest robot pose based on drive and vision data. */ + public Pose2d getLatestPose() { + return latestPose; + } + + /** Resets the odometry to a known pose. */ + public void resetPose(Pose2d pose) { + basePose = pose; + updates.clear(); + update(); + } + + /** Records a new drive movement. */ + public void addDriveData(double timestamp, Twist2d twist) { + updates.put(timestamp, new PoseUpdate(twist, new ArrayList<>())); + update(); + } + + /** Records a new set of vision updates. */ + public void addVisionData(List visionData) { + for (var timestampedVisionUpdate : visionData) { + var timestamp = timestampedVisionUpdate.timestamp(); + var visionUpdate = + new VisionUpdate(timestampedVisionUpdate.pose(), timestampedVisionUpdate.stdDevs()); + + if (updates.containsKey(timestamp)) { + // There was already an update at this timestamp, add to it + var oldVisionUpdates = updates.get(timestamp).visionUpdates(); + oldVisionUpdates.add(visionUpdate); + oldVisionUpdates.sort(VisionUpdate.compareDescStdDev); + + } else { + // Insert a new update + var prevUpdate = updates.floorEntry(timestamp); + var nextUpdate = updates.ceilingEntry(timestamp); + if (prevUpdate == null || nextUpdate == null) { + // Outside the range of existing data + return; + } + + // Create partial twists (prev -> vision, vision -> next) + var twist0 = + GeomUtil.multiplyTwist( + nextUpdate.getValue().twist(), + (timestamp - prevUpdate.getKey()) / (nextUpdate.getKey() - prevUpdate.getKey())); + var twist1 = + GeomUtil.multiplyTwist( + nextUpdate.getValue().twist(), + (nextUpdate.getKey() - timestamp) / (nextUpdate.getKey() - prevUpdate.getKey())); + + // Add new pose updates + var newVisionUpdates = new ArrayList(); + newVisionUpdates.add(visionUpdate); + newVisionUpdates.sort(VisionUpdate.compareDescStdDev); + updates.put(timestamp, new PoseUpdate(twist0, newVisionUpdates)); + updates.put( + nextUpdate.getKey(), new PoseUpdate(twist1, nextUpdate.getValue().visionUpdates())); + } + } + + // Recalculate latest pose once + update(); + } + + /** Clears old data and calculates the latest pose. */ + private void update() { + // Clear old data and update base pose + while (updates.size() > 1 + && updates.firstKey() < Timer.getFPGATimestamp() - historyLengthSecs) { + var update = updates.pollFirstEntry(); + basePose = update.getValue().apply(basePose, q); + } + + // Update latest pose + latestPose = basePose; + for (var updateEntry : updates.entrySet()) { + latestPose = updateEntry.getValue().apply(latestPose, q); + } + } + + /** + * Represents a sequential update to a pose estimate, with a twist (drive movement) and list of + * vision updates. + */ + private static record PoseUpdate(Twist2d twist, ArrayList visionUpdates) { + public Pose2d apply(Pose2d lastPose, Matrix q) { + // Apply drive twist + var pose = lastPose.exp(twist); + + // Apply vision updates + for (var visionUpdate : visionUpdates) { + // Calculate Kalman gains based on std devs + // (https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/estimator/) + Matrix visionK = new Matrix<>(Nat.N3(), Nat.N3()); + var r = new double[3]; + for (int i = 0; i < 3; ++i) { + r[i] = visionUpdate.stdDevs().get(i, 0) * visionUpdate.stdDevs().get(i, 0); + } + for (int row = 0; row < 3; ++row) { + if (q.get(row, 0) == 0.0) { + visionK.set(row, row, 0.0); + } else { + visionK.set( + row, row, q.get(row, 0) / (q.get(row, 0) + Math.sqrt(q.get(row, 0) * r[row]))); + } + } + + // Calculate twist between current and vision pose + var visionTwist = pose.log(visionUpdate.pose()); + + // Multiply by Kalman gain matrix + var twistMatrix = + visionK.times(VecBuilder.fill(visionTwist.dx, visionTwist.dy, visionTwist.dtheta)); + + // Apply twist + pose = + pose.exp( + new Twist2d(twistMatrix.get(0, 0), twistMatrix.get(1, 0), twistMatrix.get(2, 0))); + } + + return pose; + } + } + + /** Represents a single vision pose with associated standard deviations. */ + public record VisionUpdate(Pose2d pose, Matrix stdDevs) { + public static final Comparator compareDescStdDev = + (VisionUpdate a, VisionUpdate b) -> { + return -Double.compare( + a.stdDevs().get(0, 0) + a.stdDevs().get(1, 0), + b.stdDevs().get(0, 0) + b.stdDevs().get(1, 0)); + }; + } + + /** Represents a single vision pose with a timestamp and associated standard deviations. */ + public record TimestampedVisionUpdate( + double timestamp, Pose2d pose, Matrix stdDevs) {} +} diff --git a/src/main/java/lib/GeomUtil.java b/src/main/java/lib/GeomUtil.java new file mode 100644 index 00000000..21e0957f --- /dev/null +++ b/src/main/java/lib/GeomUtil.java @@ -0,0 +1,144 @@ +// Copyright (c) 2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package lib; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist2d; + +/** Geometry utilities for working with translations, rotations, transforms, and poses. */ +public class GeomUtil { + /** + * Creates a pure translating transform + * + * @param translation The translation to create the transform with + * @return The resulting transform + */ + public static Transform2d translationToTransform(Translation2d translation) { + return new Transform2d(translation, new Rotation2d()); + } + + /** + * Creates a pure translating transform + * + * @param x The x componenet of the translation + * @param y The y componenet of the translation + * @return The resulting transform + */ + public static Transform2d translationToTransform(double x, double y) { + return new Transform2d(new Translation2d(x, y), new Rotation2d()); + } + + /** + * Creates a pure rotating transform + * + * @param rotation The rotation to create the transform with + * @return The resulting transform + */ + public static Transform2d rotationToTransform(Rotation2d rotation) { + return new Transform2d(new Translation2d(), rotation); + } + + /** + * Converts a Pose2d to a Transform2d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform2d poseToTransform(Pose2d pose) { + return new Transform2d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Converts a Transform2d to a Pose2d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose2d transformToPose(Transform2d transform) { + return new Pose2d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Creates a pure translated pose + * + * @param translation The translation to create the pose with + * @return The resulting pose + */ + public static Pose2d translationToPose(Translation2d translation) { + return new Pose2d(translation, new Rotation2d()); + } + + /** + * Creates a pure rotated pose + * + * @param rotation The rotation to create the pose with + * @return The resulting pose + */ + public static Pose2d rotationToPose(Rotation2d rotation) { + return new Pose2d(new Translation2d(), rotation); + } + + /** + * Multiplies a twist by a scaling factor + * + * @param twist The twist to multiply + * @param factor The scaling factor for the twist components + * @return The new twist + */ + public static Twist2d multiplyTwist(Twist2d twist, double factor) { + return new Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor); + } + + /** + * Converts a Pose3d to a Transform3d to be used in a kinematic chain + * + * @param pose The pose that will represent the transform + * @return The resulting transform + */ + public static Transform3d pose3dToTransform3d(Pose3d pose) { + return new Transform3d(pose.getTranslation(), pose.getRotation()); + } + + /** + * Converts a Transform3d to a Pose3d to be used as a position or as the start of a kinematic + * chain + * + * @param transform The transform that will represent the pose + * @return The resulting pose + */ + public static Pose3d transform3dToPose3d(Transform3d transform) { + return new Pose3d(transform.getTranslation(), transform.getRotation()); + } + + /** + * Converts a Translation3d to a Translation2d by extracting two dimensions (X and Y). chain + * + * @param transform The original translation + * @return The resulting translation + */ + public static Translation2d translation3dTo2dXY(Translation3d translation) { + return new Translation2d(translation.getX(), translation.getY()); + } + + /** + * Converts a Translation3d to a Translation2d by extracting two dimensions (X and Z). chain + * + * @param transform The original translation + * @return The resulting translation + */ + public static Translation2d translation3dTo2dXZ(Translation3d translation) { + return new Translation2d(translation.getX(), translation.getZ()); + } +} From 6e7f21b0e937495ac7ef975e1bd4e390f353d4ed Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 8 Feb 2024 22:21:59 -0500 Subject: [PATCH 12/13] fixed build error --- .../frc/robot/subsystems/drive/DriveSubsystem.java | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index c28054ba..71a57110 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -73,6 +73,20 @@ public class DriveSubsystem extends SubsystemBase { private final VisionSubsystem[] m_cameras; + public DriveSubsystem ( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO) { + this (gyroIO, + flModuleIO, + frModuleIO, + blModuleIO, + brModuleIO, + new VisionSubsystem[]{}); + } + public DriveSubsystem( GyroIO gyroIO, ModuleIO flModuleIO, From 09424c3590795c14ff9e2bf850e4668cb09bf15c Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 8 Feb 2024 22:24:56 -0500 Subject: [PATCH 13/13] sonarqube fixes --- .../frc/robot/subsystems/drive/DriveSubsystem.java | 7 +------ .../frc/robot/subsystems/vision/VisionSubsystem.java | 4 ---- src/main/java/frc/robot/util/PoseEstimator.java | 12 +++++------- src/main/java/lib/GeomUtil.java | 2 ++ 4 files changed, 8 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 71a57110..35198a76 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -18,7 +18,6 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -28,8 +27,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -45,7 +42,6 @@ import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; -import java.util.stream.Collectors; import java.util.stream.Stream; import frc.robot.util.PoseEstimator; @@ -68,7 +64,6 @@ public class DriveSubsystem extends SubsystemBase { private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private final PoseEstimator m_poseEstimator; private Pose2d pose = new Pose2d(); - private Pose2d visionPose = new Pose2d(); private Rotation2d lastGyroRotation = new Rotation2d(); private final VisionSubsystem[] m_cameras; @@ -186,7 +181,7 @@ public void periodic() { .flatMap((VisionSubsystem camera) -> Stream.of(camera.getPose(m_poseEstimator.getLatestPose()))) .filter(Optional::isPresent) .flatMap(update -> Stream.of(update.get())) - .collect(Collectors.toList())); + .toList()); } /** diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 1fa4d1e1..cde52a21 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -12,10 +12,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.PoseEstimator; -import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; @@ -30,7 +27,6 @@ public class VisionSubsystem { private final PhotonCamera m_camera; private final Transform3d robotToCam; private PhotonPoseEstimator m_photonPoseEstimator; - private DriverStation.Alliance m_alliance; private AprilTagFieldLayout m_aprilTagFieldLayout; private final String m_name; diff --git a/src/main/java/frc/robot/util/PoseEstimator.java b/src/main/java/frc/robot/util/PoseEstimator.java index ec3de406..61c9baec 100644 --- a/src/main/java/frc/robot/util/PoseEstimator.java +++ b/src/main/java/frc/robot/util/PoseEstimator.java @@ -23,7 +23,7 @@ import lib.GeomUtil; public class PoseEstimator { - private static final double historyLengthSecs = 0.3; + private static final double HISTORY_LENGTH_SECS = 0.3; private Pose2d basePose = new Pose2d(); private Pose2d latestPose = new Pose2d(); @@ -104,7 +104,7 @@ public void addVisionData(List visionData) { private void update() { // Clear old data and update base pose while (updates.size() > 1 - && updates.firstKey() < Timer.getFPGATimestamp() - historyLengthSecs) { + && updates.firstKey() < Timer.getFPGATimestamp() - HISTORY_LENGTH_SECS) { var update = updates.pollFirstEntry(); basePose = update.getValue().apply(basePose, q); } @@ -163,11 +163,9 @@ public Pose2d apply(Pose2d lastPose, Matrix q) { /** Represents a single vision pose with associated standard deviations. */ public record VisionUpdate(Pose2d pose, Matrix stdDevs) { public static final Comparator compareDescStdDev = - (VisionUpdate a, VisionUpdate b) -> { - return -Double.compare( - a.stdDevs().get(0, 0) + a.stdDevs().get(1, 0), - b.stdDevs().get(0, 0) + b.stdDevs().get(1, 0)); - }; + (VisionUpdate a, VisionUpdate b) -> -Double.compare( + a.stdDevs().get(0, 0) + a.stdDevs().get(1, 0), + b.stdDevs().get(0, 0) + b.stdDevs().get(1, 0)); } /** Represents a single vision pose with a timestamp and associated standard deviations. */ diff --git a/src/main/java/lib/GeomUtil.java b/src/main/java/lib/GeomUtil.java index 21e0957f..ea570e48 100644 --- a/src/main/java/lib/GeomUtil.java +++ b/src/main/java/lib/GeomUtil.java @@ -18,6 +18,8 @@ /** Geometry utilities for working with translations, rotations, transforms, and poses. */ public class GeomUtil { + private GeomUtil() {} + /** * Creates a pure translating transform *