Publishing AprilTags for AdvantageScope

Technical Programming
1

I’m experimenting with publishing AprilTag data to AdvantageScope. I can publish the Pose3d objects, and they appear, but I can’t figure out how to publish the AprilTag objects (fiducial ID and Pose3d). The documentation says AprilTag is supported, but it doesn’t provide any details for how to do it.

This works for the Pose3d, but how do I set up the table to publish AprilTag objects instead of Pose3d objects?

private StructArrayPublisher<Pose3d> arrayPublisher = NetworkTableInstance.getDefault()
    .getStructArrayTopic("AprilTags", Pose3d.struct).publish();

public void periodic() {
...
    var result = simulatedCamera.getLatestResult();
    if (result.hasTargets()) {
      var tagsToPublish = result.targets.stream().map(target -> {
        return getTargetPose(robotPose, target);
      }).toArray(Pose3d[]::new);
      arrayPublisher.accept(tagsToPublish);
    }
...
}
2

Do the same thing, but use AprilTag instead of Pose3d. AprilTag (WPILib API 2024.3.1)

e.g.

private StructArrayPublisher<AprilTag> arrayPublisher = NetworkTableInstance.getDefault()
    .getStructArrayTopic("AprilTags", AprilTag.struct).publish();

And provide an AprilTag[] to arrayPublisher.accept()

3

AprilTag doesn’t have a static .struct field like Pose3d. Do I need to create my own struct or protobuf?

4

Whoops, you’re right, we haven’t implemented that in WPILib yet. You could implement your own (either struct or protobuf) pretty easily, but I’m not sure what AdvantageScope is expecting here. @jonahb55 ?

5

Thanks. That’s what didn’t make sense to me. It says it’s “supported”, but it seems like they need to document what it expects since it doesn’t work out of the box.

6

I think something like this should work for struct support (based on Pose3dStruct.java):

public class AprilTagStruct implements Struct<AprilTag> {
  @Override
  public Class<AprilTag> getTypeClass() {
    return AprilTag.class;
  }

  @Override
  public String getTypeString() {
    return "struct:AprilTag";
  }

  @Override
  public int getSize() {
    return kSizeInt8 + Pose3d.struct.getSize();
  }

  @Override
  public String getSchema() {
    return "uint8 id;Pose3d pose";
  }

  @Override
  public Struct<?>[] getNested() {
    return new Struct<?>[] {Pose3d.struct};
  }

  @Override
  public AprilTag unpack(ByteBuffer bb) {
    int id = (int) bb.get();
    Pose3d pose = Pose3d.struct.unpack(bb);
    return new AprilTag(id, pose);
  }

  @Override
  public void pack(ByteBuffer bb, AprilTag value) {
    bb.put(bb, (byte) value.getId());
    Pose3d.struct.pack(bb, value.getPose());
  }
}
7

Thanks @Peter_Johnson that was really close, I just had to fix the pack() method:

  @Override
  public void pack(ByteBuffer bb, AprilTag value) {
    bb.put((byte) value.ID);
    Pose3d.struct.pack(bb, value.pose);
  }

Works like a charm:

private StructArrayPublisher<AprilTag> arrayPublisher = NetworkTableInstance.getDefault()
    .getStructArrayTopic("AprilTags", new AprilTagStruct()).publish();

  public void periodic() {
    ...
    if (result.hasTargets()) {
      var tagsToPublish = result.targets.stream().map(target -> {
        return getTargetPose(robotPose, target);
      }).toArray(AprilTag[]::new);

      arrayPublisher.accept(tagsToPublish);
    }
    ...
  }

  private AprilTag getTargetPose(Pose2d robotPose, PhotonTrackedTarget target) {
    var camToTarget = target.getBestCameraToTarget();
    var camPose = new Pose3d(robotPose).transformBy(VisionConstants.ROBOT_TO_CAMERA_TRANSFORMS[0]);
    var targetPose = camPose.transformBy(camToTarget);
    return new AprilTag(target.getFiducialId(), targetPose);
  }
8

The expected format is based on this draft PR: [apriltag] Add initial Protobuf support to AprilTag by srimanachanta · Pull Request #5919 · wpilibsuite/allwpilib · GitHub. The only difference from what you posted is that it expects ID to be uppercase (I think it’s case sensitive?), which is consistent with how it appears in the AprilTag class and JSON format. Checking for lowercase “id” too would be a trivial change though.

9

Is AdvantageScope case sensitive? It uses lowercase “id”: AdvantageScope/src/shared/geometry.ts at main · Mechanical-Advantage/AdvantageScope · GitHub

10

That’s an internal data type. It expects the key used by the struct/protobuf to be uppercase “ID”: AdvantageScope/src/hub/tabControllers/ThreeDimensionController.ts at main · Mechanical-Advantage/AdvantageScope · GitHub

11

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.