diff --git a/src/main/kotlin/com/team4099/lib/vision/TimestampedVisionUpdate.kt b/src/main/kotlin/com/team4099/lib/vision/TimestampedVisionUpdate.kt new file mode 100644 index 00000000..9cccdc80 --- /dev/null +++ b/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, + val fromVision: Boolean = false +) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 7c2f28c2..faed7283 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/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) { + fun addVisionData(visionData: List) { swerveDrivePoseEstimator.addVisionData(visionData) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt index 4a38b1e0..21379379 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/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> = Consumer {} + var visionConsumer: Consumer> = 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> + visionConsumer: Consumer> ) { this.poseSupplier = poseSupplier this.visionConsumer = visionConsumer diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index 1eec2086..c3ee809f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/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() } - private var visionConsumer: Consumer> = Consumer {} + private var fieldFramePoseSupplier = Supplier { Pose2d() } + private var visionConsumer: Consumer> = Consumer {} private val lastFrameTimes = mutableMapOf() private val lastTagDetectionTimes = mutableMapOf() @@ -55,10 +55,10 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { } fun setDataInterfaces( - poseSupplier: Supplier, - visionConsumer: Consumer> + fieldFramePoseSupplier: Supplier, + visionConsumer: Consumer> ) { - 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() - val visionUpdates = mutableListOf() + val visionUpdates = mutableListOf() 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) ) ) diff --git a/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt b/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt index b4a4955e..a6697f37 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/FieldFrameEstimator.kt +++ b/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) { } } - /** Represents a single vision pose with a timestamp and associated standard deviations. */ - class TimestampedVisionUpdate( - val timestamp: Time, - val fieldTRobot: Pose2d, - val stdDevs: Matrix, - val fromVision: Boolean = false - ) companion object { private val HISTORY_LENGTH = 0.3.seconds } diff --git a/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt b/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt index fb840cfa..16a00a7c 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt +++ b/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) { 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) { } } - /** Represents a single vision pose with a timestamp and associated standard deviations. */ - class TimestampedVisionUpdate( - val timestamp: Time, - val pose: Pose2d, - val stdDevs: Matrix, - val fromVision: Boolean = false - ) companion object { private const val historyLengthSecs = 0.3 }