From 50dbd64c6eec614f28b6664108a310518073d0aa Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Sat, 17 Feb 2024 18:04:55 -0500 Subject: [PATCH] working camera sim --- simgui-ds.json | 25 ++++--- .../lib/logging/DoubleArrayConversions.kt | 7 +- .../com/team4099/robot2023/RobotContainer.kt | 8 +- .../config/constants/FieldConstants.kt | 16 +++- .../robot2023/subsystems/vision/Vision.kt | 9 +-- .../subsystems/vision/camera/CameraIO.kt | 17 ++--- .../vision/camera/CameraIOPhotonvision.kt | 75 +++++++++---------- .../subsystems/vision/camera/CameraIOSim.kt | 68 +++++++++++++++++ 8 files changed, 150 insertions(+), 75 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt diff --git a/simgui-ds.json b/simgui-ds.json index 97f4895e..de69ad8b 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "System Joysticks": { "window": { "visible": false @@ -20,9 +25,17 @@ "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 + }, + { + "decKey": 75, + "incKey": 74 + }, + { + "decKey": 76, + "incKey": 73 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, @@ -45,16 +58,6 @@ "povCount": 1 }, { - "axisConfig": [ - { - "decKey": 74, - "incKey": 76 - }, - { - "decKey": 73, - "incKey": 75 - } - ], "axisCount": 2, "buttonCount": 4, "buttonKeys": [ diff --git a/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt b/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt index 07f4b7cd..9a52c34f 100644 --- a/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt +++ b/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt @@ -24,9 +24,10 @@ fun Pose3d.toDoubleArray(): Array { this.x.inMeters, this.y.inMeters, this.z.inMeters, - this.rotation.x.inRadians, - this.rotation.y.inRadians, - this.rotation.z.inRadians + this.rotation.quaternion.w.inRadians, + this.rotation.quaternion.x.inMeters, + this.rotation.quaternion.y.inMeters, + this.rotation.quaternion.z.inMeters ) } diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 605cbc82..6f208425 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -28,6 +28,7 @@ import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision +import com.team4099.robot2023.subsystems.vision.camera.CameraIOSim import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.subsystems.wrist.WristIO import com.team4099.robot2023.subsystems.wrist.WristIOSim @@ -74,12 +75,7 @@ object RobotContainer { } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) - vision = - Vision( - CameraIOPhotonvision("parakeet_1"), - CameraIOPhotonvision("parakeet_2"), - CameraIOPhotonvision("parakeet_3") - ) + vision = Vision(CameraIOSim("skrt")) limelight = LimelightVision(object : LimelightVisionIO {}) intake = Intake(IntakeIOSim) feeder = Feeder(FeederIOSim) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt index 247d4573..2507e757 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -2,14 +2,28 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.apriltag.AprilTag import org.team4099.lib.geometry.Pose3d +import org.team4099.lib.geometry.Rotation3d +import org.team4099.lib.geometry.Translation3d import org.team4099.lib.units.base.feet import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.radians object FieldConstants { val fieldLength = 54.feet val fieldWidth = 26.feet - val aprilTags: List = listOf() + val aprilTags: List = + listOf( + AprilTag( + 0, + Pose3d( + Translation3d(1.0.meters, 5.0.meters, 2.0.meters), + Rotation3d(0.0.radians, 0.0.radians, 180.degrees) + ) + ) + ) val homeAprilTags: List = listOf() val wpilibAprilTags = 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 35dfd209..27116b86 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -2,8 +2,8 @@ package com.team4099.robot2023.subsystems.vision import com.team4099.lib.hal.Clock import com.team4099.lib.logging.TunableNumber -import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.lib.vision.TimestampedVisionUpdate +import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.vision.camera.CameraIO import edu.wpi.first.math.VecBuilder @@ -14,15 +14,12 @@ import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Pose3dWPILIB -import org.team4099.lib.geometry.Quaternion -import org.team4099.lib.geometry.Rotation3d import org.team4099.lib.units.base.Time import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.radians import java.util.function.Consumer import java.util.function.Supplier import kotlin.math.pow @@ -86,6 +83,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { val visionUpdates = mutableListOf() for (instance in io.indices) { + val currentPose = fieldFramePoseSupplier.get() lastFrameTimes[instance] = Clock.fpgaTime val timestamp = inputs[instance].timestamp @@ -119,7 +117,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) ) ) @@ -139,7 +137,6 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { *tagPoses.map { it.pose3d }.toTypedArray() ) - if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol Logger.recordOutput( "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt index 83aa1187..fb5f4264 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt @@ -1,5 +1,6 @@ package com.team4099.robot2023.subsystems.vision.camera +import com.team4099.lib.logging.toDoubleArray import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs import org.team4099.lib.geometry.Pose3d @@ -16,20 +17,16 @@ interface CameraIO { override fun toLog(table: LogTable?) { table?.put("timestampSeconds", timestamp.inSeconds) - table?.put("frame", frame.pose3d) + table?.put("frame", frame.toDoubleArray().toDoubleArray()) table?.put("fps", fps) - table?.put("usedTargets", usedTargets) + table?.put("usedTargets", usedTargets.toIntArray()) } override fun fromLog(table: LogTable?) { - table?.get("timestampSeconds", 0.0)?.let { - timestamp = it.seconds - } - table?.get("frame", Pose3dWPILIB())?.let { - frame = Pose3d(it) - } - table?.get("fps", 0.0) - table?.get("usedTargets", listOf()) + table?.get("timestampSeconds", 0.0)?.let { timestamp = it.seconds } + table?.get("frame", Pose3dWPILIB())?.let { frame = Pose3d(it[0]) } + table?.get("fps", 0.0).let { fps = it ?: 0.0 } + table?.get("usedTargets", IntArray(0)).let { usedTargets = it?.toList() ?: listOf() } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt index 9026d901..e327a2b5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt @@ -5,51 +5,50 @@ import org.photonvision.EstimatedRobotPose import org.photonvision.PhotonCamera import org.photonvision.PhotonPoseEstimator import org.photonvision.PhotonPoseEstimator.PoseStrategy -import org.team4099.lib.around import org.team4099.lib.geometry.Pose3d -import org.team4099.lib.geometry.Pose3dWPILIB import org.team4099.lib.geometry.Transform3dWPILIB import org.team4099.lib.units.base.Time import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.seconds import org.team4099.lib.units.micro -import org.team4099.lib.units.milli -import java.util.* - - -class CameraIOPhotonvision(private val identifier: String): CameraIO { - - private val photonEstimator: PhotonPoseEstimator - private val camera: PhotonCamera - private var lastEstTimestamp: Time = 0.0.seconds - - init { - camera = PhotonCamera(identifier) - photonEstimator = PhotonPoseEstimator( - FieldConstants.wpilibFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, Transform3dWPILIB() - ) - photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY) - +import java.util.Optional + +class CameraIOPhotonvision(private val identifier: String) : CameraIO { + + private val photonEstimator: PhotonPoseEstimator + private val camera: PhotonCamera + private var lastEstTimestamp: Time = 0.0.seconds + + init { + camera = PhotonCamera(identifier) + photonEstimator = + PhotonPoseEstimator( + FieldConstants.wpilibFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, + Transform3dWPILIB() + ) + photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY) + } + + override fun updateInputs(inputs: CameraIO.CameraInputs) { + val visionEst: Optional? = photonEstimator.update() + val poseEst = visionEst?.get()?.let { Pose3d(it.estimatedPose) } + val latestTimestamp = visionEst?.get()?.timestampSeconds?.seconds + if (latestTimestamp != null) { + if ((latestTimestamp - lastEstTimestamp).absoluteValue > 10.micro.seconds) { + inputs.fps = 1 / (latestTimestamp - lastEstTimestamp).inSeconds + lastEstTimestamp = latestTimestamp + } } - override fun updateInputs(inputs: CameraIO.CameraInputs) { - val visionEst: Optional? = photonEstimator.update() - val poseEst = visionEst?.get()?.let { Pose3d(it.estimatedPose) } - val latestTimestamp = visionEst?.get()?.timestampSeconds?.seconds - if (latestTimestamp != null) { - if ((latestTimestamp - lastEstTimestamp).absoluteValue > 10.micro.seconds) { - inputs.fps = 1/(latestTimestamp - lastEstTimestamp).inSeconds - lastEstTimestamp = latestTimestamp - } - } - - if (visionEst != null) { - inputs.usedTargets = visionEst.get().targetsUsed.map { it.fiducialId } - } + if (visionEst != null) { + inputs.usedTargets = visionEst.get().targetsUsed.map { it.fiducialId } + } - inputs.timestamp = lastEstTimestamp - if (poseEst != null) { - inputs.frame = poseEst - } + inputs.timestamp = lastEstTimestamp + if (poseEst != null) { + inputs.frame = poseEst } -} \ No newline at end of file + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt new file mode 100644 index 00000000..91dc86ed --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt @@ -0,0 +1,68 @@ +package com.team4099.robot2023.subsystems.vision.camera + +import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.toDoubleArray +import com.team4099.robot2023.config.constants.FieldConstants +import com.team4099.robot2023.config.constants.VisionConstants +import com.team4099.robot2023.util.toPose3d +import com.team4099.robot2023.util.toTransform3d +import edu.wpi.first.networktables.DoubleArraySubscriber +import edu.wpi.first.networktables.NetworkTableInstance +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.Translation2d +import org.team4099.lib.geometry.Translation3d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.radians + +class CameraIOSim(val id: String) : CameraIO { + private val poseSubscriber: DoubleArraySubscriber = + NetworkTableInstance.getDefault() + .getTable("AdvantageKit") + .getSubTable("RealOutputs") + .getDoubleArrayTopic(VisionConstants.POSE_TOPIC_NAME) + .subscribe(DoubleArray(3)) + private val errorScaleConstant = 0.05 + + override fun updateInputs(inputs: CameraIO.CameraInputs) { + val drivePose = poseSubscriber.get(DoubleArray(3)).toPose2d().toPose3d() + + inputs.timestamp = Clock.fpgaTime - (Math.random() * 0.2).seconds + val robotTtag = drivePose.relativeTo(FieldConstants.aprilTags[0].pose) + inputs.frame = robotTtag.gaussianShenanigans() + val estimatedRobotPose = + FieldConstants.aprilTags[0].pose.transformBy(inputs.frame.toTransform3d()) + + Logger.recordOutput( + "Vision/$id/simEstimatedRobotPose", + estimatedRobotPose.toPose2d().toDoubleArray().toDoubleArray() + ) + Logger.recordOutput( + "Vision/$id/simTagPose", FieldConstants.aprilTags[0].pose.toDoubleArray().toDoubleArray() + ) + inputs.fps = Math.random() * 10 + 30 + inputs.usedTargets = listOf(0) + } + + private fun DoubleArray.toPose2d(): Pose2d { + return Pose2d(Translation2d(this[0].meters, this[1].meters), this[2].radians) + } + + private fun Pose3d.gaussianShenanigans(): Pose3d { + return Pose3d( + Translation3d( + this.translation.x * (1 - Math.random() * errorScaleConstant), + this.translation.y * (1 - Math.random() * errorScaleConstant), + this.translation.z * (1 - Math.random() * errorScaleConstant) + ), + Rotation3d( + this.rotation.x * (1 - Math.random() * errorScaleConstant), + this.rotation.y * (1 - Math.random() * errorScaleConstant), + this.rotation.z * (1 - Math.random() * errorScaleConstant) + ) + ) + } +}