使用MegaTag进行本地化
- 确保已将场地地图上传到您的Limelight。
- 将pipeline 0配置为Apriltag Pipeline并启用3D功能。
- 在Limelight UI中配置Limelight相对于机器人中心的姿态。
- 参考以下代码。我们只在可见多个标 签时采用Limelight的估计。我们将测量旋转的标准差设置为一个较大的数值,因为我们可以信任IMU给出的正确机器人朝向。
- 我们始终读取botpose_wpiblue测量值,因为常见的路径规划工具采用了蓝色角落始终作为场地原点的策略。
- 如果可能,请使用megatag 2。
完整示例项目请参见此处
public void updateOdometry() {
m_poseEstimator.update(
m_gyro.getRotation2d(),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_backLeft.getPosition(),
m_backRight.getPosition()
});
boolean useMegaTag2 = true; //设置为false以使用MegaTag1
boolean doRejectUpdate = false;
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,9999999));
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(m_gyro.getRate()) > 720) // 如果我们的角速度大于每秒720度,则忽略视觉更新
{
doRejectUpdate = true;
}
if(mt2.tagCount == 0)
{
doRejectUpdate = true;
}
if(!doRejectUpdate)
{
m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7,.7,9999999));
m_poseEstimator.addVisionMeasurement(
mt2.pose,
mt2.timestampSeconds);
}
}
}