Skip to content

Commit 34e7f71

Browse files
write2shaunak-botwrite2shaunak-bot
committed
pose rejection done, tag rejection done, checks done, ts was lowk fun
1 parent 4b23ed8 commit 34e7f71

3 files changed

Lines changed: 134 additions & 80 deletions

File tree

‎src/main/kotlin/com/team4099/robot2026/config/constants/VisionConstants.kt‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,13 +27,18 @@ object VisionConstants {
2727
val CONFIDENCE_THRESHOLD = 0.75
2828
val TAG_TRUST_THRESHOLD = 0.85
2929

30+
// Pose acceptance thresholds
31+
val POSE_FIELD_BOUNDARY_TOLERANCE_METERS = 0.10
32+
val POSE_ACCEPTANCE_MAX_LINEAR_SPEED_MPS = 6.0
33+
val POSE_ACCEPTANCE_MAX_ANGULAR_SPEED_RADPS = 12.0
34+
3035
// Trust rating weights
3136
val AMBIGUITY_TRUST_RATING = 0.45
3237
val DISTANCE_TRUST_RATING = 0.35
3338
val ANGLE_TRUST_RATING = 0.15
3439
val VELOCITY_TRUST_RATING = 0.15
3540

36-
// Velocity trust sub-weights
41+
// Velocity trust
3742
val LINEAR_VELOCITY_TRUST_WEIGHT = 0.40
3843
val ANGULAR_VELOCITY_TRUST_WEIGHT = 0.60
3944

‎src/main/kotlin/com/team4099/robot2026/subsystems/vision/Vision.kt‎

Lines changed: 94 additions & 75 deletions
Original file line numberDiff line numberDiff line change
@@ -103,93 +103,112 @@ class Vision(
103103
CameraIO.DetectionPipeline.APRIL_TAG -> {
104104
val targetingTags = mutableListOf<Pair<Int, Transform3d>>()
105105
var closestTargetTag: Pair<Int, Transform3d>? = null
106-
107-
val tagTargets = inputs[instance].cameraTargets.filter { it.fiducialId != -1 }
108-
109106
val cornerData = mutableListOf<Double>()
110107

111-
for (tag in tagTargets) {
112-
if (tag.poseAmbiguity < VisionConstants.AMBIGUITY_THESHOLD) {
113-
if (DriverStation.getAlliance().isPresent) {
114-
val robotTTag = io[instance].transform.plus(Transform3d(tag.bestCameraToTarget))
115-
116-
val distanceToTarget = robotTTag.translation.norm
117-
val trustRating =
118-
io[instance].calculateTagTrust(
119-
tag, distanceToTarget.inMeters, robotTTag, chassisSpeedsSupplier.get())
120-
CustomLogger.recordDebugOutput(
121-
"Vision/${io[instance].identifier}/${tag.fiducialId}/trustRating", trustRating)
122-
123-
CustomLogger.recordDebugOutput(
124-
"Vision/${io[instance].identifier}/${tag.fiducialId}/robotDistanceToTarget",
125-
distanceToTarget.inMeters)
126-
127-
CustomLogger.recordDebugOutput(
128-
"Vision/${io[instance].identifier}/${tag.fiducialId}/tagArea", tag.area)
129-
130-
CustomLogger.recordDebugOutput(
131-
"Vision/${io[instance].identifier}/${tag.fiducialId}/numCorners",
132-
tag.detectedCorners.size)
133-
134-
CustomLogger.recordOutput(
135-
"Vision/${io[instance].identifier}/${tag.fiducialId}/robotTTag",
136-
robotTTag.transform3d)
108+
val chassisSpeeds = chassisSpeedsSupplier.get()
109+
val hasTagEstimate = inputs[instance].cameraTargets.any { it.fiducialId != -1 }
110+
val poseForAcceptance = if (hasTagEstimate) inputs[instance].frame else poseSupplier.get()
111+
val robotPoseAccepted = io[instance].calculatePoseAcceptance(chassisSpeeds, poseForAcceptance)
112+
113+
CustomLogger.recordDebugOutput(
114+
"Vision/${io[instance].identifier}/poseAcceptedForTagTrust", robotPoseAccepted)
115+
CustomLogger.recordDebugOutput(
116+
"Vision/${io[instance].identifier}/hasTagEstimate", hasTagEstimate)
117+
118+
if (robotPoseAccepted && hasTagEstimate) {
119+
io[instance].poseMeasurementConsumer(
120+
inputs[instance].frame.pose3d,
121+
inputs[instance].timestamp.inSeconds,
122+
io[instance].curStdDevs)
123+
}
137124

138-
for (corner in tag.detectedCorners) {
139-
cornerData.add(corner.x)
140-
cornerData.add(corner.y)
141-
}
142-
if (tag.fiducialId in tagIDFilter &&
143-
trustRating >= VisionConstants.TAG_TRUST_THRESHOLD) {
144-
targetingTags.add(Pair(tag.fiducialId, robotTTag))
125+
if (!robotPoseAccepted) {
126+
closestTargetingTags[instance] = null
127+
CustomLogger.recordOutput("Vision/${io[instance].identifier}/closestTargetTagID", -1)
128+
CustomLogger.recordOutput(
129+
"Vision/${io[instance].identifier}/closestTargetTagPose", Transform3dWPILIB())
130+
} else {
131+
val tagTargets = inputs[instance].cameraTargets.filter { it.fiducialId != -1 }
132+
133+
for (tag in tagTargets) {
134+
if (tag.poseAmbiguity < VisionConstants.AMBIGUITY_THESHOLD) {
135+
if (DriverStation.getAlliance().isPresent) {
136+
val robotTTag = io[instance].transform.plus(Transform3d(tag.bestCameraToTarget))
137+
138+
val distanceToTarget = robotTTag.translation.norm
139+
val trustRating =
140+
io[instance].calculateTagTrust(
141+
tag, distanceToTarget.inMeters, robotTTag, chassisSpeeds)
142+
CustomLogger.recordDebugOutput(
143+
"Vision/${io[instance].identifier}/${tag.fiducialId}/trustRating", trustRating)
144+
145+
CustomLogger.recordDebugOutput(
146+
"Vision/${io[instance].identifier}/${tag.fiducialId}/robotDistanceToTarget",
147+
distanceToTarget.inMeters)
148+
149+
CustomLogger.recordDebugOutput(
150+
"Vision/${io[instance].identifier}/${tag.fiducialId}/tagArea", tag.area)
151+
152+
CustomLogger.recordDebugOutput(
153+
"Vision/${io[instance].identifier}/${tag.fiducialId}/numCorners",
154+
tag.detectedCorners.size)
155+
156+
CustomLogger.recordOutput(
157+
"Vision/${io[instance].identifier}/${tag.fiducialId}/robotTTag",
158+
robotTTag.transform3d)
159+
160+
for (corner in tag.detectedCorners) {
161+
cornerData.add(corner.x)
162+
cornerData.add(corner.y)
163+
}
164+
if (tag.fiducialId in tagIDFilter && trustRating) {
165+
targetingTags.add(Pair(tag.fiducialId, robotTTag))
166+
}
145167
}
146168
}
169+
}
147170

148-
closestTargetTag = targetingTags.minByOrNull { it.second.translation.norm }
149-
150-
closestTargetingTags[instance] = closestTargetTag
171+
closestTargetTag = targetingTags.minByOrNull { it.second.translation.norm }
172+
closestTargetingTags[instance] = closestTargetTag
151173

152-
CustomLogger.recordDebugOutput(
153-
"Vision/${io[instance].identifier}/cornerDetections", cornerData.toDoubleArray())
174+
CustomLogger.recordDebugOutput(
175+
"Vision/${io[instance].identifier}/cornerDetections", cornerData.toDoubleArray())
154176

155-
CustomLogger.recordOutput(
156-
"Vision/${io[instance].identifier}/closestTargetTagID",
157-
closestTargetTag?.first ?: -1)
177+
CustomLogger.recordOutput(
178+
"Vision/${io[instance].identifier}/closestTargetTagID",
179+
closestTargetTag?.first ?: -1)
158180

159-
CustomLogger.recordOutput(
160-
"Vision/${io[instance].identifier}/closestTargetTagPose",
161-
closestTargetTag?.second?.transform3d ?: Transform3dWPILIB())
162-
}
181+
CustomLogger.recordOutput(
182+
"Vision/${io[instance].identifier}/closestTargetTagPose",
183+
closestTargetTag?.second?.transform3d ?: Transform3dWPILIB())
184+
}
163185

164-
closestTargetTagAcrossCams =
165-
if (closestTargetingTags[0]?.first != closestTargetingTags[1]?.first) {
166-
closestTargetingTags.minByOrNull {
167-
it.value?.second?.translation?.norm ?: 1000000.meters
168-
}
169-
} else {
170-
mapOf(cameraPreference to closestTargetingTags[cameraPreference]).minByOrNull {
171-
it.value?.second?.translation?.norm ?: 1000000.meters
172-
}
186+
closestTargetTagAcrossCams =
187+
if (closestTargetingTags[0]?.first != closestTargetingTags[1]?.first) {
188+
closestTargetingTags.minByOrNull { it.value?.second?.translation?.norm ?: 1000000.meters }
189+
} else {
190+
mapOf(cameraPreference to closestTargetingTags[cameraPreference]).minByOrNull {
191+
it.value?.second?.translation?.norm ?: 1000000.meters
173192
}
193+
}
174194

175-
CustomLogger.recordOutput(
176-
"Vision/ClosestTargetTagAcrossAllCams/TagID",
177-
closestTargetTagAcrossCams?.value?.first ?: -1)
178-
179-
CustomLogger.recordDebugOutput(
180-
"Vision/ClosestTargetTagAcrossAllCams/TargetTagPose",
181-
closestTargetTagAcrossCams?.value?.second?.transform3d ?: Transform3dWPILIB())
182-
183-
if (closestTargetTagAcrossCams?.key != null &&
184-
closestTargetTagAcrossCams?.value != null) {
185-
lastTrigVisionUpdate =
186-
TimestampedTrigVisionUpdate(
187-
inputs[closestTargetTagAcrossCams?.key ?: 0].timestamp,
188-
closestTargetTagAcrossCams?.value?.first ?: -1,
189-
Transform3d(
190-
closestTargetTagAcrossCams?.value?.second?.translation ?: Translation3d(),
191-
closestTargetTagAcrossCams?.value?.second?.rotation ?: Rotation3d()))
192-
}
195+
CustomLogger.recordOutput(
196+
"Vision/ClosestTargetTagAcrossAllCams/TagID",
197+
closestTargetTagAcrossCams?.value?.first ?: -1)
198+
199+
CustomLogger.recordDebugOutput(
200+
"Vision/ClosestTargetTagAcrossAllCams/TargetTagPose",
201+
closestTargetTagAcrossCams?.value?.second?.transform3d ?: Transform3dWPILIB())
202+
203+
if (closestTargetTagAcrossCams?.key != null &&
204+
closestTargetTagAcrossCams?.value != null) {
205+
lastTrigVisionUpdate =
206+
TimestampedTrigVisionUpdate(
207+
inputs[closestTargetTagAcrossCams?.key ?: 0].timestamp,
208+
closestTargetTagAcrossCams?.value?.first ?: -1,
209+
Transform3d(
210+
closestTargetTagAcrossCams?.value?.second?.translation ?: Translation3d(),
211+
closestTargetTagAcrossCams?.value?.second?.rotation ?: Rotation3d()))
193212
}
194213
}
195214
CameraIO.DetectionPipeline.OBJECT_DETECTION -> {

‎src/main/kotlin/com/team4099/robot2026/subsystems/vision/camera/CameraIO.kt‎

Lines changed: 34 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package com.team4099.robot2026.subsystems.vision.camera
22

3+
import com.team4099.robot2026.config.constants.FieldConstants
34
import com.team4099.robot2026.config.constants.VisionConstants
45
import edu.wpi.first.math.Matrix
56
import edu.wpi.first.math.VecBuilder
@@ -19,6 +20,7 @@ import org.photonvision.targeting.PhotonTrackedTarget
1920
import org.team4099.lib.geometry.Pose3d
2021
import org.team4099.lib.geometry.Pose3dWPILIB
2122
import org.team4099.lib.geometry.Rotation3d
23+
import org.team4099.lib.kinematics.ChassisSpeeds
2224
import org.team4099.lib.units.base.inMeters
2325
import org.team4099.lib.units.base.inSeconds
2426
import org.team4099.lib.units.base.seconds
@@ -42,11 +44,41 @@ interface CameraIO {
4244
var curStdDevs: Matrix<N4?, N1?>
4345
val photonEstimator: PhotonPoseEstimator
4446

47+
fun calculatePoseAcceptance(
48+
chassisSpeeds: ChassisSpeeds,
49+
robotPose: Pose3d,
50+
fieldBoundaryToleranceMeters: Double = VisionConstants.POSE_FIELD_BOUNDARY_TOLERANCE_METERS,
51+
maxLinearSpeedForAcceptanceMetersPerSecond: Double =
52+
VisionConstants.POSE_ACCEPTANCE_MAX_LINEAR_SPEED_MPS,
53+
maxAngularSpeedForAcceptanceRadPerSec: Double =
54+
VisionConstants.POSE_ACCEPTANCE_MAX_ANGULAR_SPEED_RADPS
55+
): Boolean {
56+
val xMeters = robotPose.x.inMeters
57+
val yMeters = robotPose.y.inMeters
58+
val zMeters = robotPose.z.inMeters
59+
60+
// rejects robot below field z<0
61+
if (zMeters < 0.0) return false
62+
63+
val minBound = -fieldBoundaryToleranceMeters
64+
val maxX = FieldConstants.fieldLength.inMeters + fieldBoundaryToleranceMeters
65+
val maxY = FieldConstants.fieldWidth.inMeters + fieldBoundaryToleranceMeters
66+
val insideFieldBounds = xMeters in minBound..maxX && yMeters in minBound..maxY
67+
68+
if (!insideFieldBounds) return false
69+
70+
val linearSpeed = kotlin.math.hypot(chassisSpeeds.vx.inMetersPerSecond, chassisSpeeds.vy.inMetersPerSecond)
71+
val angularSpeed = kotlin.math.abs(chassisSpeeds.omega.inRadiansPerSecond)
72+
73+
return linearSpeed <= maxLinearSpeedForAcceptanceMetersPerSecond &&
74+
angularSpeed <= maxAngularSpeedForAcceptanceRadPerSec
75+
}
76+
4577
fun calculateTagTrustScore(
4678
tag: PhotonTrackedTarget,
4779
distanceToTarget: Double,
4880
robotTTag: org.team4099.lib.geometry.Transform3d,
49-
chassisSpeeds: org.team4099.lib.kinematics.ChassisSpeeds
81+
chassisSpeeds: ChassisSpeeds
5082
): Double {
5183
// 1. Ambiguity trust (0-1, higher is better)
5284
val ambiguityTrust = (1.0 - tag.poseAmbiguity).coerceIn(0.0, 1.0)
@@ -109,7 +141,7 @@ interface CameraIO {
109141
tag: PhotonTrackedTarget,
110142
distanceToTarget: Double,
111143
robotTTag: org.team4099.lib.geometry.Transform3d,
112-
chassisSpeeds: org.team4099.lib.kinematics.ChassisSpeeds,
144+
chassisSpeeds: ChassisSpeeds,
113145
minTrustThreshold: Double = VisionConstants.TAG_TRUST_THRESHOLD
114146
): Boolean {
115147
return calculateTagTrustScore(tag, distanceToTarget, robotTTag, chassisSpeeds) >= minTrustThreshold
@@ -208,8 +240,6 @@ interface CameraIO {
208240
inputs.frame = Pose3d(poseEst)
209241

210242
updateEstimationStdDevs(visionEst, result.getTargets())
211-
212-
poseMeasurementConsumer(poseEst, visionEst.get().timestampSeconds, curStdDevs)
213243
}
214244
}
215245
}

0 commit comments

Comments
 (0)