使用MegaTag2实现机器人定位
MegaTag2于2024年推出，是一个基于AprilTag的精确且无歧义的移动机器人定位器。它的设计目标如下：
- 消除姿态歧义问题并提高对图像/角点噪声的鲁棒性
- 基于单个标签提供优秀的姿态估计，无论视角如何
- 提高对物理AprilTag放 置不准确的鲁棒性
- 减少获得良好姿态估计结果所需的滤波处理
MegaTag2在任何距离下都能通过单个标签提供出色的结果。 这意味着完全可以只关注相关且在容差范围内的标签，过滤掉所有其他标签。如果标签位置不正确，可以使用随MegaTag2一起引入的动态过滤功能将其过滤掉。
int[] validIDs = {3,4};
LimelightHelpers.SetFiducialIDFiltersOverride("limelight", validIDs);
与MT1不同，MT2假定你知道机器人的航向(yaw)。可选地，MegaTag2接受完整的机器人方向和角速度。
要求：
- 你的Limelight的机器人空间姿态已在webUI或通过API配置
- 已上传场地地图(.fmap)
- 在机器人端代码中每帧调用LimelightHelpers.SetRobotOrientation(robotYawInDegrees,0,0,0,0,0)
- SetRobotOrientation假定以中心（参见地图生成器）或蓝方角落为原点。逆时针为正，0度 -> 在FRC中面向红方联盟墙
NetworkTables键值：
- botpose_orb_wpiblue
- botpose_orb_wpired
- botpose_orb
JSON键值：
- botpose_orb_wpiblue
- botpose_orb_wpired
- botpose_orb
- (每个基准目标) t6r_fs_orb - 仅基于此标签使用megatag2的机器人在场地空间中的姿态（非多标签）
注意在这个高度歧义的单标签情况下MegaTag2（红色机器人）和Megatag（蓝色机器人）的区别
- 金色圆柱/红色机器人：未经过滤的Megatag2 botpose
- 黄色圆柱：未经过滤的单标签Megatag2 botposes
- 白色圆柱/蓝色机器人：MegaTag1 Botpose
- 绿色圆柱：单个标签的机器人姿态(MT1)
- 蓝色圆柱：单个标签机器人姿态的平均值(MT1)
信息
在2024年，大部分WPILib生态系统过渡到单一原点坐标系统。 在2023年，你的坐标系原点会根据联盟颜色改变。
对于2024年及以后，你的坐标系原点应始终是"蓝方"原点。FRC团队应始终使用botpose_orb_wpiblue进行姿态相关功能
使用WPILib的姿态估计器
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);
}
配置Limelight的机器人空间姿态
LL Forward、LL Right和LL Up表示如果你站在机器人的位置上，沿机器人前进、右侧和向上向量的距离（单位：米）。 LL Roll、Pitch和Yaw表示Limelight的旋转角度（单位：度）。你可以修改这些值，并在3D查看器中观察Limelight 3D模型的变化。 Limelight在内部使用此配置将相机空间中的目标姿态转换为场地空间中的机器人姿态。