Pose Estimation with Limelight

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

@@ -1,9 +1,14 @@
 package com.team4099.lib.vision
 
+import com.team4099.robot2023.subsystems.limelight.CoordinatePair
 import org.photonvision.targeting.TargetCorner
 
 data class TargetCorner(val x: Double, val y: Double) {
   constructor(
     photonTargetCorner: TargetCorner
   ) : this(photonTargetCorner.x, photonTargetCorner.y) {}
+
+  fun toCoordinatePair(): CoordinatePair {
+    return CoordinatePair(x, y)
+  }
 }

src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt

@@ -25,4 +25,16 @@ object VisionConstants {
     )
 
   val CAMERA_NAMES = listOf("northstar")
+
+  object Limelight {
+    val HORIZONTAL_FOV = 59.6.degrees
+    val VERITCAL_FOV = 45.7.degrees
+    val LL_TRANSFORM =
+      Transform3d(
+        Translation3d((11.760.inches - 1.25.inches), 7.3125.inches, 29.33.inches),
+        Rotation3d(180.degrees, 0.degrees, 0.degrees)
+      )
+    val RES_WIDTH = 320
+    val RES_HEIGHT = 240 // no clue what these numbers should be but usnig these for now
+  }
 }

src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt

@@ -53,7 +53,7 @@ interface ElevatorIO {
       table?.put("elevatorFollowerTempCelsius", followerTempCelcius.inCelsius)
     }
 
-    override fun fromLog(table: LogTable) {
+    override fun fromLog(table: LogTable?) {
       table?.getDouble("elevatorPositionInches", elevatorPosition.inInches)?.let {
         elevatorPosition = it.inches
       }

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

@@ -0,0 +1,3 @@
+package com.team4099.robot2023.subsystems.limelight
+
+data class CoordinatePair(val x: Double, val y: Double)

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

@@ -0,0 +1,163 @@
+package com.team4099.robot2023.subsystems.limelight
+
+import com.team4099.lib.vision.TargetCorner
+import com.team4099.robot2023.config.constants.VisionConstants
+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
+import org.littletonrobotics.junction.Logger
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Rotation3d
+import org.team4099.lib.geometry.Transform3d
+import org.team4099.lib.geometry.Translation3d
+import org.team4099.lib.units.base.Length
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.tan
+import java.util.function.Consumer
+import java.util.function.Supplier
+
+class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
+  val inputs = LimelightVisionIO.LimelightVisionIOInputs()
+
+  var poseSupplier: Supplier<Pose2d> = Supplier { Pose2d() }
+  var visionConsumer: Consumer<List<PoseEstimator.TimestampedVisionUpdate>> = Consumer {}
+
+  // i think we need this for camera project to irl coordinates
+  val vpw = (2.0 * (VisionConstants.Limelight.HORIZONTAL_FOV / 2).tan)
+  val vph = (2.0 * (VisionConstants.Limelight.VERITCAL_FOV / 2).tan)
+
+  override fun periodic() {
+    io.updateInputs(inputs)
+    Logger.getInstance().processInputs("LimelightVision", inputs)
+
+    val currentPose: Pose2d = poseSupplier.get()
+
+    val visibleNodes = mutableListOf<Pose3d>()
+
+    // mathematically figure out which nodes we're looking at based on limelight fov
+
+    // process transform outputs from LL and get the target corners
+    if (inputs.validReading) {
+      // idk how to do this part
+
+      // this is adding where we think they are,, not where they actually are
+      visibleNodes.add(
+        solveTargetPositionFromCameraOutput(
+          currentPose,
+          pixelPosition = CoordinatePair(0.0, 0.0),
+          VisionConstants.Limelight.LL_TRANSFORM,
+          targetHeight = 0.0.meters
+        )
+      )
+    }
+  }
+
+  // based off of angles
+  fun solveTargetPositionFromAngularOutput(
+    tx: Angle,
+    ty: Angle,
+    currentPose: Pose2d,
+    cameraTransform: Transform3d,
+    targetHeight: Length
+  ): Pose3d {
+    val horizontalAngleFromRobot = tx + cameraTransform.rotation.x
+    val verticalAngleFromRobot = ty + cameraTransform.rotation.y
+
+    // rotation from robot to the target in frame
+    val rotationFromTargetToRobot =
+      Rotation3d(0.0.degrees, verticalAngleFromRobot, horizontalAngleFromRobot)
+
+    val xDistanceFromTargetToRobot = (targetHeight - cameraTransform.x) / verticalAngleFromRobot.tan
+    val yDistanceFromTargetToRobot = xDistanceFromTargetToRobot / horizontalAngleFromRobot.tan
+
+    val translationFromTargetToRobot =
+      Translation3d(
+        xDistanceFromTargetToRobot,
+        yDistanceFromTargetToRobot,
+        targetHeight - cameraTransform.z
+      )
+
+    return currentPose
+      .toPose3d()
+      .transformBy(cameraTransform)
+      .transformBy(Transform3d(translationFromTargetToRobot, rotationFromTargetToRobot))
+  }
+
+  // based off of pixel coordinates
+  private fun pixelCoordsToNormalizedPixelCoords(pixelCoords: CoordinatePair): CoordinatePair {
+    // we're defining a coordinate pair to be where 0,0 is (horizontal fov / 2, vertical fov / 2)
+    // and 1,1 is 1 pixel shifted both vertically and horizontally
+    // note that pixel coordinates is returned where 0,0 is upper left, pos x is down, and pos y is
+    // right but we want pos x to be right and pos y to be up
+    return CoordinatePair(
+      1 / (VisionConstants.Limelight.RES_WIDTH / 2) *
+        (pixelCoords.x - VisionConstants.Limelight.RES_WIDTH - 0.5),
+      1 / (VisionConstants.Limelight.RES_HEIGHT / 2) *
+        (pixelCoords.x - VisionConstants.Limelight.RES_HEIGHT - 0.5)
+    )
+  }
+
+  // need to make sure that all the corners are from the same target before passing into this
+  // function
+  fun solveTargetPositionFromCameraOutput(
+    currentPose: Pose2d,
+    pixelPosition: CoordinatePair,
+    cameraTransform: Transform3d,
+    targetHeight: Length
+  ): Pose3d {
+    val normalizedCoordinates = pixelCoordsToNormalizedPixelCoords(pixelPosition)
+
+    // figure out which way this target is facing using yaw of robot and yaw of camera
+    val targetRotation =
+      if (currentPose.rotation.rotateBy(cameraTransform.rotation.z) in 0.degrees..180.degrees) {
+        // we are looking at a blue node which is facing towards 0 degrees
+        0.0.degrees
+      } else {
+        // we are looking at a red node which is facing 180 degrees
+        180.degrees
+      }
+
+    // i think we wanna do what is there above because we are getting absolute position of the node
+    // relative to the origin
+    //    val targetRotation =
+    // currentPose.rotation.rotateBy(VisionConstants.Limelight.LL_TRANSFORM.rotation.z).rotateBy(180.degrees) // assuming we are looking at the tape head on
+
+    val cartesianPoseOfTarget =
+      Pose3d(
+        currentPose.x + (vpw / 2 * normalizedCoordinates.x).meters,
+        currentPose.y + (vph / 2 * normalizedCoordinates.y).meters,
+        targetHeight,
+        Rotation3d(0.0.degrees, 0.0.degrees, targetRotation)
+      )
+
+    return cartesianPoseOfTarget
+  }
+
+  // assume top left, top right, bottom left, bottom right
+  fun centerOfRectangle(corners: List<TargetCorner>): CoordinatePair? {
+    var xPos: Double? = null
+    var yPos: Double? = null
+    if (corners.size == 4) {
+      xPos = corners.map { it.x }.average()
+      yPos = corners.map { it.y }.average()
+    }
+
+    return if (xPos != null && yPos != null) {
+      CoordinatePair(xPos, yPos)
+    } else {
+      null
+    }
+  }
+
+  fun setDataInterfaces(
+    poseSupplier: Supplier<Pose2d>,
+    visionConsumer: Consumer<List<PoseEstimator.TimestampedVisionUpdate>>
+  ) {
+    this.poseSupplier = poseSupplier
+    this.visionConsumer = visionConsumer
+  }
+}

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

@@ -0,0 +1,33 @@
+package com.team4099.robot2023.subsystems.limelight
+
+import com.team4099.robot2023.util.LimelightHelpers
+import org.littletonrobotics.junction.LogTable
+import org.littletonrobotics.junction.inputs.LoggableInputs
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.degrees
+
+interface LimelightVisionIO {
+
+  class LimelightVisionIOInputs : LoggableInputs {
+    var timestamp = 0.0.seconds
+    var fps = 0.0
+    var validReading = false
+    var angle = 0.degrees
+    var corners = listOf<Pair<Double, Double>>()
+    var retroTargets = listOf<LimelightHelpers.LimelightTarget_Retro>()
+
+    override fun fromLog(table: LogTable?) {
+      TODO("Not yet implemented")
+    }
+
+    override fun toLog(table: LogTable?) {
+      TODO("Not yet implemented")
+    }
+  }
+
+  fun updateInputs(inputs: LimelightVisionIOInputs) {}
+
+  fun setPipeline(pipelineIndex: Int) {}
+
+  fun setLeds(enabled: Boolean) {}
+}

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

@@ -0,0 +1,65 @@
+package com.team4099.robot2023.subsystems.limelight
+
+import com.team4099.lib.hal.Clock
+import com.team4099.robot2023.util.LimelightHelpers
+import edu.wpi.first.networktables.NetworkTableEntry
+import edu.wpi.first.networktables.NetworkTableInstance
+import org.team4099.lib.units.base.inMilliseconds
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.milli
+
+class LimelightVisionIOReal : LimelightVisionIO {
+
+  val limelightName = "limelight"
+
+  private val ledEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(limelightName).getEntry("ledMode")
+  private val pipelineEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(limelightName).getEntry("pipeline")
+  private val validEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(limelightName).getEntry("tv")
+  private val latencyEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(limelightName).getEntry("tl")
+  private val captureLatencyEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(limelightName).getEntry("cl")
+  private val dataEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(limelightName).getEntry("tcornxy")
+  private val angleEntry: NetworkTableEntry =
+    NetworkTableInstance.getDefault().getTable(limelightName).getEntry("tx")
+
+  override fun updateInputs(inputs: LimelightVisionIO.LimelightVisionIOInputs) {
+    val totalLatency =
+      (
+        latencyEntry.getDouble(0.0).milli.seconds +
+          captureLatencyEntry.getDouble(0.0).milli.seconds
+        )
+
+    inputs.timestamp = Clock.realTimestamp - totalLatency
+    inputs.angle = angleEntry.getDouble(0.0).degrees
+    inputs.fps = 1000 / totalLatency.inMilliseconds
+
+    val entry = dataEntry.getDoubleArray(DoubleArray(0))
+    val xCoords = mutableListOf<Double>()
+    val yCoords = mutableListOf<Double>()
+    for (coordinateIndex in entry.indices) {
+      if (coordinateIndex % 2 == 0) {
+        xCoords.add(entry[coordinateIndex])
+      } else {
+        yCoords.add(entry[coordinateIndex])
+      }
+    }
+    inputs.corners = xCoords.zip(yCoords).toList()
+
+    inputs.retroTargets =
+      LimelightHelpers.getLatestResults(limelightName).targetingResults.targets_Retro.toList()
+  }
+
+  override fun setPipeline(pipelineIndex: Int) {
+    pipelineEntry.setInteger(pipelineIndex.toLong())
+  }
+
+  override fun setLeds(enabled: Boolean) {
+    ledEntry.setBoolean(enabled)
+  }
+}

src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt

@@ -328,8 +328,6 @@ class Superstructure(
                 }
               }
             }
-            is SuperstructureRequest.SingleSubstationIntakePrep ->
-              SuperstructureStates.SINGLE_SUBSTATION_INTAKE_PREP
             is SuperstructureRequest.Tuning -> SuperstructureStates.TUNING
             else -> currentState
           }

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

@@ -2,6 +2,10 @@ package com.team4099.robot2023.util
 
 import com.team4099.robot2023.config.constants.FieldConstants
 import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.units.base.inMeters
+import kotlin.math.pow
+import kotlin.math.sqrt
 
 fun Pose2d.isOnInnerSideOfChargeStation(): Boolean {
   val normalizedPose = AllianceFlipUtil.apply(this)
@@ -18,3 +22,36 @@ fun Pose2d.isAboveMiddleOfChargeStation(): Boolean {
         ) / 2
       )
 }
+
+fun Pose3d.findClosestPose(vararg pose3d: Pose3d): Pose3d {
+  var closestPose = pose3d[0]
+  if (pose3d.size > 1) {
+    for (pose in pose3d) {
+      if (this.closerToInTranslation(pose, closestPose) == pose) {
+        closestPose = pose
+      }
+    }
+  }
+
+  return closestPose
+}
+
+fun Pose3d.closerToInTranslation(pose1: Pose3d, pose2: Pose3d): Pose3d {
+  val distToPose1 =
+    sqrt(
+      (this.x - pose1.x).inMeters.pow(2) +
+        (this.y - pose1.y).inMeters.pow(2) +
+        (this.z - pose1.z).inMeters.pow(2)
+    )
+  val distToPose2 =
+    sqrt(
+      (this.x - pose2.x).inMeters.pow(2) +
+        (this.y - pose2.y).inMeters.pow(2) +
+        (this.z - pose2.z).inMeters.pow(2)
+    )
+  if (distToPose1 < distToPose2) {
+    return pose1
+  } else {
+    return pose2
+  }
+}

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

@@ -1,6 +1,14 @@
 package com.team4099.robot2023.util
 
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Rotation3d
 import org.team4099.lib.geometry.Twist2d
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.angle
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inRotation2ds
 
 /**
  * Multiplies a twist by a scaling factor
@@ -12,3 +20,11 @@ import org.team4099.lib.geometry.Twist2d
 fun multiplyTwist(twist: Twist2d, factor: Double): Twist2d {
   return Twist2d(twist.dx * factor, twist.dy * factor, twist.dtheta * factor)
 }
+
+fun Pose2d.toPose3d(): Pose3d {
+  return Pose3d(this.x, this.y, 0.0.meters, Rotation3d(0.0.degrees, 0.0.degrees, this.rotation))
+}
+
+fun Angle.rotateBy(angle: Angle): Angle {
+  return this.inRotation2ds.rotateBy(angle.inRotation2ds).angle
+}