search

LimelightVision / limelightlib-wpijava

48 stars 16 forks source link

Add MegaTag2 Checker to PoseEstimate #29

Open JosephTLockwood opened 2 months ago

JosephTLockwood commented 2 months ago

Many people use MegaTag1 when the robot is disabled (to get a starting pose) and MegaTag2 when enabled, which can lead to the following code structure.

LimelightHelpers.SetRobotOrientation(
    limelightName, swerveStateSupplier.get().Pose.getRotation().getDegrees(), 0, 0, 0, 0, 0);
PoseEstimate mt =
    DriverStation.isEnabled()
        ? LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName)
        : LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName);

followed by

double thetaStdDev = DriverStation.isEnabled() ? calculateThetaStdDev(mt) : 9999999;

This can lead to issues where DriverStation.isEnabled() returns different values (if on different threads), and MegaTag2 rotation is trusted. Due to the current MegaTag2 rotation should not be trusted if used. This would allow you to change the second block of code to.

double thetaStdDev = mt.isMegaTag2 ? calculateThetaStdDev(mt) : 9999999;

This pull request also adds a small helper function to check if a PoseEstimate is valid.