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

Use ReadQueue for PhotonCamera timestamps (#1316)

Browse Source 
This removes the extra GetLastChange call to keep everything properly
atomic.

Closes #1303
This commit is contained in:
Matt
2024-08-04 14:23:46 -04:00
committed by GitHub
parent 37e9d40762
commit 67463a020a
29 changed files with 1057 additions and 1614 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -44,8 +44,8 @@ import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.Optional;
@@ -137,10 +137,12 @@ public class PhotonCamera implements AutoCloseable {
cameraTable
.getRawTopic("rawBytes")
.subscribe(
"rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
resultSubscriber =
new PacketSubscriber<>(
rawBytesEntry, PhotonPipelineResult.serde, new PhotonPipelineResult());
"rawBytes",
new byte[] {},
PubSubOption.periodic(0.01),
PubSubOption.sendAll(true),
PubSubOption.pollStorage(20));
resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.serde);
driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
@@ -176,21 +178,52 @@ public class PhotonCamera implements AutoCloseable {
}
/**
* Returns the latest pipeline result.
*
* @return The latest pipeline result.
* The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().
* Calling this function clears the internal FIFO queue, and multiple calls to
* getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to
* call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so
* make sure to call this frequently enough to avoid old results being discarded, too!
*/
public List<PhotonPipelineResult> getAllUnreadResults() {
List<PhotonPipelineResult> ret = new ArrayList<>();
var changes = resultSubscriber.getAllChanges();
// TODO: NT4 timestamps are still not to be trusted. But it's the best we can do until we can
// make time sync more reliable.
for (var c : changes) {
var result = c.value;
result.setRecieveTimestampMicros(c.timestamp);
ret.add(result);
}
return ret;
}
/**
* Returns the latest pipeline result. This is simply the most recent result recieved 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
* results, or provide duplicate ones!
*/
@Deprecated(since = "2024", forRemoval = true)
public PhotonPipelineResult getLatestResult() {
verifyVersion();
var ret = resultSubscriber.get();
// Set the timestamp of the result.
// getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
ret.setRecieveTimestampMicros(RobotController.getFPGATime());
if (ret.timestamp == 0) return new PhotonPipelineResult();
// Return result.
return ret;
var result = ret.value;
// Set the timestamp of the result. Since PacketSubscriber doesn't realize that the result
// 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);
return result;
}
/**

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

@@ -90,7 +90,6 @@ public class PhotonPoseEstimator {
private TargetModel tagModel = TargetModel.kAprilTag36h11;
private PoseStrategy primaryStrategy;
private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY;
private final PhotonCamera camera;
private Transform3d robotToCamera;
private Pose3d lastPose;
@@ -107,31 +106,21 @@ public class PhotonPoseEstimator {
* Coordinate System</a>. Note that setting the origin of this layout object will affect the
* results from this class.
* @param strategy The strategy it should use to determine the best pose.
* @param camera PhotonCamera
* @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
* robot ➔ camera) in the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* Coordinate System</a>.
*/
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags,
PoseStrategy strategy,
PhotonCamera camera,
Transform3d robotToCamera) {
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.primaryStrategy = strategy;
this.camera = camera;
this.robotToCamera = robotToCamera;
HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount);
InstanceCount++;
}
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
this(fieldTags, strategy, null, robotToCamera);
}
/** Invalidates the pose cache. */
private void invalidatePoseCache() {
poseCacheTimestampSeconds = -1;
@@ -288,31 +277,8 @@ public class PhotonPoseEstimator {
}
/**
* Poll data from the configured cameras and update the estimated position of the robot. Returns
* empty if:
*
* <ul>
* <li>New data has not been received since the last call to {@code update()}.
* <li>No targets were found from the camera
* <li>There is no camera set
* </ul>
*
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> update() {
if (camera == null) {
DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false);
return Optional.empty();
}
PhotonPipelineResult cameraResult = camera.getLatestResult();
return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
}
/**
* Updates the estimated position of the robot. Returns empty if:
* Updates the estimated position of the robot, assuming no camera calibration is required for the
* selected strategy. Returns empty if:
*
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
@@ -320,13 +286,15 @@ public class PhotonPoseEstimator {
* <li>No targets were found in the pipeline results.
* </ul>
*
* Will report a warning if strategy is multi-tag-on-rio, but camera calibration data is not
* provided
*
* @param cameraResult The latest pipeline result from the camera
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty());
return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
return update(cameraResult, Optional.empty(), Optional.empty());
}
/**
@@ -338,10 +306,10 @@ public class PhotonPoseEstimator {
* <li>No targets were found in the pipeline results.
* </ul>
*
* @param cameraMatrix Camera calibration data that can be used in the case of no assigned
* PhotonCamera.
* @param distCoeffs Camera calibration data that can be used in the case of no assigned
* PhotonCamera
* @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
@@ -378,7 +346,7 @@ public class PhotonPoseEstimator {
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs,
PoseStrategy strat) {
Optional<EstimatedRobotPose> estimatedPose;
Optional<EstimatedRobotPose> estimatedPose = Optional.empty();
switch (strat) {
case LOWEST_AMBIGUITY:
estimatedPose = lowestAmbiguityStrategy(cameraResult);
@@ -397,10 +365,20 @@ public class PhotonPoseEstimator {
estimatedPose = averageBestTargetsStrategy(cameraResult);
break;
case MULTI_TAG_PNP_ON_RIO:
estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
if (cameraMatrix.isEmpty()) {
DriverStation.reportWarning(
"Camera matrix is empty for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
} else if (distCoeffs.isEmpty()) {
DriverStation.reportWarning(
"Camera matrix is empty for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
} else {
estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
}
break;
case MULTI_TAG_PNP_ON_COPROCESSOR:
estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs);
estimatedPose = multiTagOnCoprocStrategy(cameraResult);
break;
default:
DriverStation.reportError(
@@ -415,10 +393,7 @@ public class PhotonPoseEstimator {
return estimatedPose;
}
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N8, N1>> distCoeffsOpt) {
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
if (result.getMultiTagResult().estimatedPose.isPresent) {
var best_tf = result.getMultiTagResult().estimatedPose.best;
var best =
@@ -433,7 +408,8 @@ public class PhotonPoseEstimator {
result.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
} else {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
// We can nver fall back on another multitag strategy
return update(result, Optional.empty(), Optional.empty(), this.multiTagFallbackStrategy);
}
}

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

@@ -81,7 +81,7 @@ public class PhotonCameraSim implements AutoCloseable {
private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest;
private final AprilTagFieldLayout tagLayout =
AprilTagFields.kDefaultField.loadAprilTagLayoutField();
AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
// video stream simulation
private final CvSource videoSimRaw;