Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Auto-generate packet dataclasses with Jinja (#1374)

Browse Source
This commit is contained in:
Matt
2024-08-31 13:44:19 -04:00
committed by GitHub
parent c19d54c633
commit 169595e56e
140 changed files with 4445 additions and 2097 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -142,7 +142,7 @@ public class PhotonCamera implements AutoCloseable {
PubSubOption.periodic(0.01),
PubSubOption.sendAll(true),
PubSubOption.pollStorage(20));
resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.serde);
resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.photonStruct);
driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
@@ -193,7 +193,7 @@ public class PhotonCamera implements AutoCloseable {
// make time sync more reliable.
for (var c : changes) {
var result = c.value;
result.setRecieveTimestampMicros(c.timestamp);
result.setReceiveTimestampMicros(c.timestamp);
ret.add(result);
}
@@ -201,7 +201,7 @@ public class PhotonCamera implements AutoCloseable {
}
/**
* Returns the latest pipeline result. This is simply the most recent result recieved via NT.
* Returns the latest pipeline result. This is simply the most recent result Received via NT.
* Calling this multiple times will always return the most recent result.
*
* <p>Replaced by {@link #getAllUnreadResults()} over getLatestResult, as this function can miss
@@ -221,7 +221,7 @@ public class PhotonCamera implements AutoCloseable {
// contains a thing with time knowledge, set it here.
// getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
// TODO: NT4 time sync is Not To Be Trusted, we should do something else?
result.setRecieveTimestampMicros(ret.timestamp);
result.setReceiveTimestampMicros(ret.timestamp);
return result;
}
@@ -411,9 +411,20 @@ public class PhotonCamera implements AutoCloseable {
"PhotonVision coprocessor at path " + path + " is not sending new data.", true);
}
// Check for version. Warn if the versions aren't aligned.
String versionString = versionEntry.get("");
if (!versionString.isEmpty() && !PhotonVersion.versionMatches(versionString)) {
// Check mdef UUID
String local_uuid = PhotonPipelineResult.photonStruct.getInterfaceUUID();
String remote_uuid = resultSubscriber.getInterfaceUUID();
if (remote_uuid == null || remote_uuid.isEmpty()) {
// not connected yet?
DriverStation.reportWarning(
"PhotonVision coprocessor at path "
+ path
+ " has note reported a message interface UUID - is your coprocessor's camera started?",
true);
} else if (!local_uuid.equals(remote_uuid)) {
// Error on a verified version mismatch
// But stay silent otherwise
@@ -439,8 +450,14 @@ public class PhotonCamera implements AutoCloseable {
var versionMismatchMessage =
"Photon version "
+ PhotonVersion.versionString
+ " (message definition version "
+ local_uuid
+ ")"
+ " does not match coprocessor version "
+ versionString
+ " (message definition version "
+ remote_uuid
+ ")"
+ "!";
DriverStation.reportError(versionMismatchMessage, false);
throw new UnsupportedOperationException(versionMismatchMessage);

8
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -394,8 +394,8 @@ public class PhotonPoseEstimator {
}
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
if (result.getMultiTagResult().estimatedPose.isPresent) {
var best_tf = result.getMultiTagResult().estimatedPose.best;
if (result.getMultiTagResult().isPresent()) {
var best_tf = result.getMultiTagResult().get().estimatedPose.best;
var best =
new Pose3d()
.plus(best_tf) // field-to-camera
@@ -427,11 +427,11 @@ public class PhotonPoseEstimator {
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent)
if (!pnpResult.isPresent())
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
var best =
new Pose3d()
.plus(pnpResult.best) // field-to-camera
.plus(pnpResult.get().best) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(

33
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -55,9 +55,9 @@ import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
/**
* A handle for simulating {@link PhotonCamera} values. Processing simulated targets through this
@@ -420,14 +420,15 @@ public class PhotonCameraSim implements AutoCloseable {
// projected target can't be detected, skip to next
if (!(canSeeCorners(noisyTargetCorners) && areaPercent >= minTargetAreaPercent)) continue;
var pnpSim = new PNPResult();
var pnpSim = new PnpResult();
if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP
pnpSim =
OpenCVHelp.solvePNP_SQUARE(
prop.getIntrinsics(),
prop.getDistCoeffs(),
tgt.getModel().vertices,
noisyTargetCorners);
OpenCVHelp.solvePNP_SQPNP(
prop.getIntrinsics(),
prop.getDistCoeffs(),
tgt.getModel().vertices,
noisyTargetCorners)
.get();
}
detectableTgts.add(
@@ -519,13 +520,13 @@ public class PhotonCameraSim implements AutoCloseable {
} else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);
// calculate multitag results
var multitagResult = new MultiTargetPNPResult();
Optional<MultiTargetPNPResult> multitagResult = Optional.empty();
// TODO: Implement ATFL subscribing in backend
// var tagLayout = cam.getAprilTagFieldLayout();
var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
List<Integer> usedIDs =
visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList());
List<Short> usedIDs =
visibleLayoutTags.stream().map(t -> (short) t.ID).sorted().collect(Collectors.toList());
var pnpResult =
VisionEstimation.estimateCamPosePNP(
prop.getIntrinsics(),
@@ -533,7 +534,10 @@ public class PhotonCameraSim implements AutoCloseable {
detectableTgts,
tagLayout,
TargetModel.kAprilTag36h11);
multitagResult = new MultiTargetPNPResult(pnpResult, usedIDs);
if (pnpResult.isPresent()) {
multitagResult = Optional.of(new MultiTargetPNPResult(pnpResult.get(), usedIDs));
}
}
// sort target order
@@ -550,7 +554,7 @@ public class PhotonCameraSim implements AutoCloseable {
now,
detectableTgts,
multitagResult);
ret.setRecieveTimestampMicros(now);
ret.setReceiveTimestampMicros(now);
return ret;
}
@@ -573,9 +577,10 @@ public class PhotonCameraSim implements AutoCloseable {
* @param receiveTimestamp The (sim) timestamp when this result was read by NT in microseconds
*/
public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimestamp) {
ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp);
ts.latencyMillisEntry.set(result.metadata.getLatencyMillis(), receiveTimestamp);
ts.resultPublisher.set(result, result.getPacketSize());
// Results are now dynamically sized, so let's guess 1024 bytes is big enough
ts.resultPublisher.set(result, 1024);
boolean hasTargets = result.hasTargets();
ts.hasTargetEntry.set(hasTargets, receiveTimestamp);