Limelight Pose Estimator Crashes

Technical Programming
1

Hey I’ve been trying to get pose estimation with limelight to work, but I’m running into a weird issue. Whenever the robot is turned on my code crashes due to the tagCount method returning null. However, if I redeploy code that issue no longer occurs and localization seems to work as intended. I haven’t seen anyone else with this problem and we obviously won’t be able to redeploy code every time we turn the robot on at events. Any help would be great. My code is based on the megatag examples on the limelight docs.

public Swerve(){

        m_poseEstimator = new SwerveDrivePoseEstimator(
            Constants.Swerve.swerveKinematics,
            gyro.getRotation2d(),
            new SwerveModulePosition[] {
                mSwerveMods[0].getPosition(),
                mSwerveMods[1].getPosition(),
                mSwerveMods[2].getPosition(),
                mSwerveMods[3].getPosition()
            },
            new Pose2d(),
            VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
            VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
}

public void updatePose(){

        m_poseEstimator.update(
            gyro.getRotation2d(),
            new SwerveModulePosition[] {
                mSwerveMods[0].getPosition(),
                mSwerveMods[1].getPosition(),
                mSwerveMods[2].getPosition(),
                mSwerveMods[3].getPosition()
            }
        );
        
        if(useMegaTag2 == false)
        {
        LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
        
          if(mt1.tagCount == 1 && mt1.rawFiducials.length == 1)
          {
            if(mt1.rawFiducials[0].ambiguity > .7)
            {
              doRejectUpdate = true;
            }
            if(mt1.rawFiducials[0].distToCamera > 3)
            {
              doRejectUpdate = true;
            }
          }
          if(mt1.tagCount == 0)
          {
            doRejectUpdate = true;
          }

          if(!doRejectUpdate)
          {
            m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5,.5,.999999));
            m_poseEstimator.addVisionMeasurement(
                mt1.pose, 
                mt1.timestampSeconds);
          }

        }

        else if (useMegaTag2 == true)
        {
          LimelightHelpers.SetRobotOrientation("limelight", m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0);
          LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
          if(Math.abs(gyro.getRate()) > 720) // if our angular velocity is greater than 720 degrees per second, ignore vision updates
          {
            doRejectUpdate = true;
          }
          if(mt2.tagCount == 0)
          {
            doRejectUpdate = true;
          }
          if(!doRejectUpdate)
          {
            m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5,.5,9999999));
            m_poseEstimator.addVisionMeasurement(
                mt2.pose,
                mt2.timestampSeconds);
          }
        }
    }

The updatePose() method is called periodically.

2

is the tag count or the pose estimate null? ive seen example code that checks if(mt1 != null), i assume theres a chance for getBotPoseEstimate calls to return null if theres no estimate, mabe while the limelight is still booting up

2 Likes
3

This is likely the issue.

What error are you actually getting when the code crashes?

4

Reading Stacktraces — FIRST Robotics Competition documentation has additional information on how to take the next steps to interpret the error you see during the code crash. It won’t tell you why your code is wrong, but it will point to the specific line of code that triggers it. From there, you can work backward to figure out the true root cause.

5

After a bit of messing around it seems like you are correct. I added a check for if getBotPoseEstimate is null and its working now. Thanks for the help!

1 Like