Merge branch 'vision_tx'

Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java

@@ -34,38 +34,34 @@ public final class SwerveEstimator {
  private boolean ignoreVision;
 
  public SwerveEstimator(FieldInfo field) {
-// tagTracker = new TagTrackerInput(
-// field,
-// new TagTrackerInput.CameraInfo( // 16 ft + 1
-// "front",
-// new Pose3d(new Translation3d(0.34, -0.225, 0.2), new Rotation3d(Math.PI, Math.toRadians(90-67), 0))),
-// new TagTrackerInput.CameraInfo(
-// "zoom",
-// new Pose3d(new Translation3d(0.235, 0.18, 0.395), new Rotation3d(0, Math.toRadians(16.88), 0)))
-// // new TagTrackerInput.CameraInfo(
-// // "back",
-// // new Pose3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)))
-// );
-
-double halfFrameL = 0.77 / 2;
-double halfFrameW = 0.695 / 2;
-tagTracker = new TagTrackerInput(
- field,
- new TagTrackerInput.CameraInfo( // 16 ft + 1
- "front",
- new Pose3d(new Translation3d(-halfFrameL + 0.695, -halfFrameW + 0.12, 0.19), new Rotation3d(Math.PI, Math.toRadians(90-67), 0))),
-new TagTrackerInput.CameraInfo(
-"zoom",
-new Pose3d(new Translation3d(halfFrameL - 0.16, halfFrameW - 0.133, 0.39), new Rotation3d(0, Math.toRadians(16.88), 0)))
-// new TagTrackerInput.CameraInfo(
-// "back",
-// new Pose3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0)))
-);
-
- // tagTracker = new TagTrackerInput(field, new TagTrackerInput.CameraInfo("zoom", new Pose3d(new Translation3d(0, 0, 0.39), new Rotation3d(0, Math.toRadians(16.88), 0))));
- // tagTracker = new TagTrackerInput(
- // field, 
- // new TagTrackerInput.CameraInfo("zoom", new Transform3d(new Pose3d(new Translation3d(0, 0, -0.39), new Rotation3d(0, Math.toRadians(16.88), 0)), new Pose3d())));
+ double halfFrameL = 0.77 / 2;
+ double halfFrameW = 0.695 / 2;
+
+ tagTracker = new TagTrackerInput(
+ field,
+
+ new TagTrackerInput.CameraInfo(
+ "front",
+ (camPose) -> {
+ return camPose
+ // Compensate for mounting angle
+// .transformBy(new Transform3d(new Translation3d(), new Rotation3d(Math.PI, Math.toRadians(90-67), 0)))
+ .transformBy(new Transform3d(new Translation3d(), new Rotation3d(Math.PI, 0, 0)))
+ .transformBy(new Transform3d(new Translation3d(), new Rotation3d(0, Math.toRadians(90-67), 0)))
+ // Compensate for mounting position
+ .transformBy(new Transform3d(new Translation3d(halfFrameL - 0.046, -halfFrameW + 0.12, 0.19).unaryMinus(), new Rotation3d()));
+ }),
+
+ new TagTrackerInput.CameraInfo(
+ "zoom",
+ (camPose) -> {
+ return camPose
+ // Compensate for mounting angle
+ .transformBy(new Transform3d(new Translation3d(), new Rotation3d(0, Math.toRadians(16.88), 0)))
+ // Compensate for mounting position
+ .transformBy(new Transform3d(new Translation3d(halfFrameL - 0.16, halfFrameW - 0.133, 0.39).unaryMinus(), new Rotation3d()));
+ })
+ );
 
  latestPose = new Pose2d();
  basePose = new Pose2d();

Robot/src/main/java/com/swrobotics/robot/subsystems/tagtracker/TagTrackerCamera.java

@@ -9,6 +9,7 @@
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
+import java.util.function.Function;
 
 public final class TagTrackerCamera {
  public static final class PoseEstimate {
@@ -86,21 +87,23 @@ public String toString() {
  }
 
  private final String name;
- private final Transform3d toRobotTransform;
+// private final Transform3d toRobotTransform;
+ private final Function<Pose3d, Pose3d> cameraToRobot;
 
  private final TagTrackerCameraIO io;
  private final TagTrackerCameraIO.Inputs inputs;
 
- public TagTrackerCamera(String name, NetworkTable table, Transform3d toRobotTransform) {
+ public TagTrackerCamera(String name, NetworkTable table, Function<Pose3d, Pose3d> cameraToRobot) {
  this.name = name;
- this.toRobotTransform = toRobotTransform;
+// this.toRobotTransform = toRobotTransform;
+ this.cameraToRobot = cameraToRobot;
 
  io = new NTCameraIO(table);
  inputs = new TagTrackerCameraIO.Inputs();
  }
 
- public Transform3d getToRobotTransform() {
- return toRobotTransform;
+ public Function<Pose3d, Pose3d> getToRobotTransform() {
+ return cameraToRobot;
  }
 
  public List<EstimateInput> getEstimates() {

Robot/src/main/java/com/swrobotics/robot/subsystems/tagtracker/TagTrackerInput.java

@@ -13,15 +13,18 @@
 
 import java.util.ArrayList;
 import java.util.List;
+import java.util.function.Function;
 
 public final class TagTrackerInput {
  public static final class CameraInfo {
  public final String name;
- public final Pose3d robotRelPose;
+ public final Function<Pose3d, Pose3d> cameraToRobot;
+// public final Pose3d robotRelPose;
 
- public CameraInfo(String name, Pose3d robotRelPose) {
+ public CameraInfo(String name, Function<Pose3d, Pose3d> cameraToRobot) {
  this.name = name;
- this.robotRelPose = robotRelPose;
+ this.cameraToRobot = cameraToRobot;
+// this.robotRelPose = robotRelPose;
  }
  }
 
@@ -60,7 +63,7 @@ public TagTrackerInput(FieldInfo field, CameraInfo... infos) {
  cameras[i] = new TagTrackerCamera(
  info.name,
  camerasTable.getSubTable(info.name),
- GeometryUtil.toTransform(info.robotRelPose).inverse());
+ info.cameraToRobot);
  }
  }
 
@@ -79,7 +82,8 @@ public List<VisionUpdate> getNewUpdates() {
  if (input.poseB == null) {
  // Only one pose available, camera sees multiple tags
  cameraPose = input.poseA.pose;
- robotPose3d = cameraPose.transformBy(camera.getToRobotTransform());
+ robotPose3d = camera.getToRobotTransform().apply(cameraPose);
+// robotPose3d = cameraPose.transformBy(camera.getToRobotTransform());
  } else {
  // Two poses available (one tag), choose the better one.
  // Pose is only chosen if it's significantly better than the
@@ -93,7 +97,8 @@ else if (errB < errA * AMBIGUITY_THRESHOLD)
  cameraPose = input.poseB.pose;
 
  if (cameraPose != null)
- robotPose3d = cameraPose.transformBy(camera.getToRobotTransform());
+ robotPose3d = camera.getToRobotTransform().apply(cameraPose);
+// robotPose3d = cameraPose.transformBy(camera.getToRobotTransform());
  }
 
  // Skip frame if no pose was good