001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.apriltag;
006
007import com.fasterxml.jackson.annotation.JsonAutoDetect;
008import com.fasterxml.jackson.annotation.JsonCreator;
009import com.fasterxml.jackson.annotation.JsonIgnore;
010import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
011import com.fasterxml.jackson.annotation.JsonProperty;
012import com.fasterxml.jackson.databind.ObjectMapper;
013import edu.wpi.first.math.geometry.Pose3d;
014import edu.wpi.first.math.geometry.Rotation3d;
015import edu.wpi.first.math.geometry.Translation3d;
016import java.io.IOException;
017import java.io.InputStream;
018import java.io.InputStreamReader;
019import java.nio.file.Path;
020import java.util.ArrayList;
021import java.util.HashMap;
022import java.util.List;
023import java.util.Map;
024import java.util.Objects;
025import java.util.Optional;
026
027/**
028 * Class for representing a layout of AprilTags on a field and reading them from a JSON format.
029 *
030 * <p>The JSON format contains two top-level objects, "tags" and "field". The "tags" object is a
031 * list of all AprilTags contained within a layout. Each AprilTag serializes to a JSON object
032 * containing an ID and a Pose3d. The "field" object is a descriptor of the size of the field in
033 * meters with "width" and "length" values. This is to account for arbitrary field sizes when
034 * transforming the poses.
035 *
036 * <p>Pose3ds in the JSON are measured using the normal FRC coordinate system, NWU with the origin
037 * at the bottom-right corner of the blue alliance wall. {@link #setOrigin(OriginPosition)} can be
038 * used to change the poses returned from {@link AprilTagFieldLayout#getTagPose(int)} to be from the
039 * perspective of a specific alliance.
040 *
041 * <p>Tag poses represent the center of the tag, with a zero rotation representing a tag that is
042 * upright and facing away from the (blue) alliance wall (that is, towards the opposing alliance).
043 */
044@JsonIgnoreProperties(ignoreUnknown = true)
045@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
046public class AprilTagFieldLayout {
047  public enum OriginPosition {
048    kBlueAllianceWallRightSide,
049    kRedAllianceWallRightSide,
050  }
051
052  private final Map<Integer, AprilTag> m_apriltags = new HashMap<>();
053
054  @JsonProperty(value = "field")
055  private FieldDimensions m_fieldDimensions;
056
057  private Pose3d m_origin;
058
059  /**
060   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
061   *
062   * @param path Path of the JSON file to import from.
063   * @throws IOException If reading from the file fails.
064   */
065  public AprilTagFieldLayout(String path) throws IOException {
066    this(Path.of(path));
067  }
068
069  /**
070   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
071   *
072   * @param path Path of the JSON file to import from.
073   * @throws IOException If reading from the file fails.
074   */
075  public AprilTagFieldLayout(Path path) throws IOException {
076    AprilTagFieldLayout layout =
077        new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class);
078    m_apriltags.putAll(layout.m_apriltags);
079    m_fieldDimensions = layout.m_fieldDimensions;
080    setOrigin(OriginPosition.kBlueAllianceWallRightSide);
081  }
082
083  /**
084   * Construct a new AprilTagFieldLayout from a list of {@link AprilTag} objects.
085   *
086   * @param apriltags List of {@link AprilTag}.
087   * @param fieldLength Length of the field the layout is representing in meters.
088   * @param fieldWidth Width of the field the layout is representing in meters.
089   */
090  public AprilTagFieldLayout(List<AprilTag> apriltags, double fieldLength, double fieldWidth) {
091    this(apriltags, new FieldDimensions(fieldLength, fieldWidth));
092  }
093
094  @JsonCreator
095  private AprilTagFieldLayout(
096      @JsonProperty(required = true, value = "tags") List<AprilTag> apriltags,
097      @JsonProperty(required = true, value = "field") FieldDimensions fieldDimensions) {
098    // To ensure the underlying semantics don't change with what kind of list is passed in
099    for (AprilTag tag : apriltags) {
100      m_apriltags.put(tag.ID, tag);
101    }
102    m_fieldDimensions = fieldDimensions;
103    setOrigin(OriginPosition.kBlueAllianceWallRightSide);
104  }
105
106  /**
107   * Returns a List of the {@link AprilTag AprilTags} used in this layout.
108   *
109   * @return The {@link AprilTag AprilTags} used in this layout.
110   */
111  @JsonProperty("tags")
112  public List<AprilTag> getTags() {
113    return new ArrayList<>(m_apriltags.values());
114  }
115
116  /**
117   * Sets the origin based on a predefined enumeration of coordinate frame origins. The origins are
118   * calculated from the field dimensions.
119   *
120   * <p>This transforms the Pose3d objects returned by {@link #getTagPose(int)} to return the
121   * correct pose relative to a predefined coordinate frame.
122   *
123   * @param origin The predefined origin
124   */
125  @JsonIgnore
126  public void setOrigin(OriginPosition origin) {
127    switch (origin) {
128      case kBlueAllianceWallRightSide:
129        setOrigin(new Pose3d());
130        break;
131      case kRedAllianceWallRightSide:
132        setOrigin(
133            new Pose3d(
134                new Translation3d(m_fieldDimensions.fieldLength, m_fieldDimensions.fieldWidth, 0),
135                new Rotation3d(0, 0, Math.PI)));
136        break;
137      default:
138        throw new IllegalArgumentException("Unsupported enum value");
139    }
140  }
141
142  /**
143   * Sets the origin for tag pose transformation.
144   *
145   * <p>This transforms the Pose3d objects returned by {@link #getTagPose(int)} to return the
146   * correct pose relative to the provided origin.
147   *
148   * @param origin The new origin for tag transformations
149   */
150  @JsonIgnore
151  public void setOrigin(Pose3d origin) {
152    m_origin = origin;
153  }
154
155  /**
156   * Gets an AprilTag pose by its ID.
157   *
158   * @param ID The ID of the tag.
159   * @return The pose corresponding to the ID passed in or an empty optional if a tag with that ID
160   *     was not found.
161   */
162  @SuppressWarnings("ParameterName")
163  public Optional<Pose3d> getTagPose(int ID) {
164    AprilTag tag = m_apriltags.get(ID);
165    if (tag == null) {
166      return Optional.empty();
167    }
168    return Optional.of(tag.pose.relativeTo(m_origin));
169  }
170
171  /**
172   * Serializes a AprilTagFieldLayout to a JSON file.
173   *
174   * @param path The path to write to.
175   * @throws IOException If writing to the file fails.
176   */
177  public void serialize(String path) throws IOException {
178    serialize(Path.of(path));
179  }
180
181  /**
182   * Serializes a AprilTagFieldLayout to a JSON file.
183   *
184   * @param path The path to write to.
185   * @throws IOException If writing to the file fails.
186   */
187  public void serialize(Path path) throws IOException {
188    new ObjectMapper().writeValue(path.toFile(), this);
189  }
190
191  /**
192   * Deserializes a field layout from a resource within a internal jar file.
193   *
194   * <p>Users should use {@link AprilTagFields#loadAprilTagLayoutField()} to load official layouts
195   * and {@link #AprilTagFieldLayout(String)} for custom layouts.
196   *
197   * @param resourcePath The absolute path of the resource
198   * @return The deserialized layout
199   * @throws IOException If the resource could not be loaded
200   */
201  public static AprilTagFieldLayout loadFromResource(String resourcePath) throws IOException {
202    try (InputStream stream = AprilTagFieldLayout.class.getResourceAsStream(resourcePath);
203        InputStreamReader reader = new InputStreamReader(stream)) {
204      return new ObjectMapper().readerFor(AprilTagFieldLayout.class).readValue(reader);
205    }
206  }
207
208  @Override
209  public boolean equals(Object obj) {
210    if (obj instanceof AprilTagFieldLayout) {
211      var other = (AprilTagFieldLayout) obj;
212      return m_apriltags.equals(other.m_apriltags) && m_origin.equals(other.m_origin);
213    }
214    return false;
215  }
216
217  @Override
218  public int hashCode() {
219    return Objects.hash(m_apriltags, m_origin);
220  }
221
222  @JsonIgnoreProperties(ignoreUnknown = true)
223  @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
224  private static class FieldDimensions {
225    @SuppressWarnings("MemberName")
226    @JsonProperty(value = "length")
227    public double fieldLength;
228
229    @SuppressWarnings("MemberName")
230    @JsonProperty(value = "width")
231    public double fieldWidth;
232
233    @JsonCreator()
234    FieldDimensions(
235        @JsonProperty(required = true, value = "length") double fieldLength,
236        @JsonProperty(required = true, value = "width") double fieldWidth) {
237      this.fieldLength = fieldLength;
238      this.fieldWidth = fieldWidth;
239    }
240  }
241}