LimelightVision / limelightlib-wpijava

48 stars 16 forks source link

Add timestamp to pose estimate #22

Closed agasser closed 8 months ago

agasser commented 11 months ago

This feature implements #21

I do not have access to a Limelight right now, so this is untested.

agasser commented 11 months ago

Here's an example how usage would look:

var estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue("");
poseEstimator.addVisionMeasurement(estimate.pose, estimate.timestampSeconds);

Without this PR, the usage would look something like this:

var poseEntry = LimelightHelpers.getLimelightNTTableEntry("limelight", "botpose_wpiblue");
var poseArray = poseEntry.getDoubleArray(new double[0]);
var timestamp = poseEntry.getLastChange() / 1e6 - poseArray[6] / 1e3;

var pose = new Pose2d(
    new Translation2d(poseArray[0], poseArray[1]),
    new Rotation2d(Units.degreesToRadians(poseArray[5])));

poseEstimator.addVisionMeasurement(pose, timestamp);
sunboyan666 commented 9 months ago

cool!