Pull TimestampedVisionUpdate out of pose estimator (#23)

Topic: refactor-vision-update
Relative: field-frame-estimator
Reviewers: saraansh, rithvik, pranav

src/main/kotlin/com/team4099/lib/vision/TimestampedVisionUpdate.kt

@@ -0,0 +1,15 @@
+package com.team4099.lib.vision
+
+import edu.wpi.first.math.Matrix
+import edu.wpi.first.math.numbers.N1
+import edu.wpi.first.math.numbers.N3
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.units.base.Time
+
+/** Represents a single vision pose with a timestamp and associated standard deviations. */
+data class TimestampedVisionUpdate(
+  val timestamp: Time,
+  val fieldTRobot: Pose2d,
+  val stdDevs: Matrix<N3, N1>,
+  val fromVision: Boolean = false
+)

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt

@@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.drivetrain.drive
 
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.lib.vision.TimestampedVisionUpdate
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.config.constants.VisionConstants
@@ -635,7 +636,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
     }
   }
 
-  fun addVisionData(visionData: List<PoseEstimator.TimestampedVisionUpdate>) {
+  fun addVisionData(visionData: List<TimestampedVisionUpdate>) {
     swerveDrivePoseEstimator.addVisionData(visionData)
   }
 

src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt

@@ -2,9 +2,9 @@ package com.team4099.robot2023.subsystems.limelight
 
 import com.team4099.lib.logging.TunableNumber
 import com.team4099.lib.vision.TargetCorner
+import com.team4099.lib.vision.TimestampedVisionUpdate
 import com.team4099.robot2023.config.constants.VisionConstants
 import com.team4099.robot2023.util.LimelightReading
-import com.team4099.robot2023.util.PoseEstimator
 import com.team4099.robot2023.util.rotateBy
 import com.team4099.robot2023.util.toPose3d
 import edu.wpi.first.wpilibj2.command.SubsystemBase
@@ -33,7 +33,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
   val inputs = LimelightVisionIO.LimelightVisionIOInputs()
 
   var poseSupplier: () -> Pose2d = { Pose2d() }
-  var visionConsumer: Consumer<List<PoseEstimator.TimestampedVisionUpdate>> = Consumer {}
+  var visionConsumer: Consumer<List<TimestampedVisionUpdate>> = Consumer {}
 
   // i think we need this for camera project to irl coordinates
   val vpw = (2.0 * (VisionConstants.Limelight.HORIZONTAL_FOV / 2).tan)
@@ -217,7 +217,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
 
   fun setDataInterfaces(
     poseSupplier: () -> Pose2d,
-    visionConsumer: Consumer<List<PoseEstimator.TimestampedVisionUpdate>>
+    visionConsumer: Consumer<List<TimestampedVisionUpdate>>
   ) {
     this.poseSupplier = poseSupplier
     this.visionConsumer = visionConsumer

src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt

@@ -2,9 +2,9 @@ package com.team4099.robot2023.subsystems.vision
 
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.TunableNumber
+import com.team4099.lib.vision.TimestampedVisionUpdate
 import com.team4099.robot2023.config.constants.VisionConstants
 import com.team4099.robot2023.subsystems.vision.camera.CameraIO
-import com.team4099.robot2023.util.PoseEstimator
 import edu.wpi.first.math.VecBuilder
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj2.command.SubsystemBase
@@ -43,8 +43,8 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
 
   private val thetaStdDev = TunableNumber("Vision/thetaStdDev", thetaStdDevCoefficient)
 
-  private var poseSupplier = Supplier<Pose2d> { Pose2d() }
-  private var visionConsumer: Consumer<List<PoseEstimator.TimestampedVisionUpdate>> = Consumer {}
+  private var fieldFramePoseSupplier = Supplier<Pose2d> { Pose2d() }
+  private var visionConsumer: Consumer<List<TimestampedVisionUpdate>> = Consumer {}
   private val lastFrameTimes = mutableMapOf<Int, Time>()
   private val lastTagDetectionTimes = mutableMapOf<Int, Time>()
 
@@ -55,10 +55,10 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
   }
 
   fun setDataInterfaces(
-    poseSupplier: Supplier<Pose2d>,
-    visionConsumer: Consumer<List<PoseEstimator.TimestampedVisionUpdate>>
+    fieldFramePoseSupplier: Supplier<Pose2d>,
+    visionConsumer: Consumer<List<TimestampedVisionUpdate>>
   ) {
-    this.poseSupplier = poseSupplier
+    this.fieldFramePoseSupplier = fieldFramePoseSupplier
     this.visionConsumer = visionConsumer
   }
 
@@ -80,9 +80,9 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
       Logger.processInputs("Vision/${VisionConstants.CAMERA_NAMES[instance]}", inputs[instance])
     }
 
-    var currentPose: Pose2d = poseSupplier.get()
+    var fieldTCurrentRobotEstimate: Pose2d = fieldFramePoseSupplier.get()
     val robotPoses = mutableListOf<Pose2d>()
-    val visionUpdates = mutableListOf<PoseEstimator.TimestampedVisionUpdate>()
+    val visionUpdates = mutableListOf<TimestampedVisionUpdate>()
 
     for (instance in io.indices) {
 
@@ -172,8 +172,8 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
 
               val robotPose1 = cameraPose1.transformBy(cameraPoses[instance].inverse()).toPose2d()
 
-              if (robotPose0.rotation.minus(currentPose.rotation).absoluteValue <
-                robotPose1.rotation.minus(currentPose.rotation).absoluteValue
+              if (robotPose0.rotation.minus(fieldTCurrentRobotEstimate.rotation).absoluteValue <
+                robotPose1.rotation.minus(fieldTCurrentRobotEstimate.rotation).absoluteValue
               ) {
                 cameraPose = cameraPose0
                 robotPose = robotPose0
@@ -193,7 +193,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
           continue
         }
 
-        if ((robotPose.rotation - currentPose.rotation).absoluteValue > 7.degrees &&
+        if ((robotPose.rotation - fieldTCurrentRobotEstimate.rotation).absoluteValue > 7.degrees &&
           DriverStation.isEnabled()
         ) {
           continue
@@ -219,7 +219,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
         val thetaStdDev = thetaStdDev.get() * averageDistance.inMeters.pow(2) / tagPoses.size
 
         visionUpdates.add(
-          PoseEstimator.TimestampedVisionUpdate(
+          TimestampedVisionUpdate(
             timestamp, robotPose, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev)
           )
         )

src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt

@@ -2,6 +2,7 @@ package com.team4099.robot2023.util
 
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.math.asTransform2d
+import com.team4099.lib.vision.TimestampedVisionUpdate
 import edu.wpi.first.math.Matrix
 import edu.wpi.first.math.Nat
 import edu.wpi.first.math.VecBuilder
@@ -204,13 +205,6 @@ class FieldFrameEstimator(stateStdDevs: Matrix<N3?, N1?>) {
     }
   }
 
-  /** Represents a single vision pose with a timestamp and associated standard deviations. */
-  class TimestampedVisionUpdate(
-    val timestamp: Time,
-    val fieldTRobot: Pose2d,
-    val stdDevs: Matrix<N3, N1>,
-    val fromVision: Boolean = false
-  )
   companion object {
     private val HISTORY_LENGTH = 0.3.seconds
   }

src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt

@@ -1,5 +1,6 @@
 package com.team4099.robot2023.util
 
+import com.team4099.lib.vision.TimestampedVisionUpdate
 import edu.wpi.first.math.Matrix
 import edu.wpi.first.math.Nat
 import edu.wpi.first.math.VecBuilder
@@ -10,7 +11,6 @@ import org.littletonrobotics.junction.Logger
 import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.geometry.Pose2dWPILIB
 import org.team4099.lib.geometry.Twist2d
-import org.team4099.lib.units.base.Time
 import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.base.meters
@@ -49,7 +49,7 @@ class PoseEstimator(stateStdDevs: Matrix<N3?, N1?>) {
       val timestamp: Double = timestampedVisionUpdate.timestamp.inSeconds
       val visionUpdate =
         VisionUpdate(
-          timestampedVisionUpdate.pose,
+          timestampedVisionUpdate.fieldTRobot,
           timestampedVisionUpdate.stdDevs,
           timestampedVisionUpdate.fromVision
         )
@@ -189,13 +189,6 @@ class PoseEstimator(stateStdDevs: Matrix<N3?, N1?>) {
     }
   }
 
-  /** Represents a single vision pose with a timestamp and associated standard deviations. */
-  class TimestampedVisionUpdate(
-    val timestamp: Time,
-    val pose: Pose2d,
-    val stdDevs: Matrix<N3, N1>,
-    val fromVision: Boolean = false
-  )
   companion object {
     private const val historyLengthSecs = 0.3
   }