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.io.UncheckedIOException;
020import java.nio.file.Path;
021import java.util.ArrayList;
022import java.util.HashMap;
023import java.util.List;
024import java.util.Map;
025import java.util.Objects;
026import java.util.Optional;
027
028/**
029 * Class for representing a layout of AprilTags on a field and reading them from a JSON format.
030 *
031 * <p>The JSON format contains two top-level objects, "tags" and "field". The "tags" object is a
032 * list of all AprilTags contained within a layout. Each AprilTag serializes to a JSON object
033 * containing an ID and a Pose3d. The "field" object is a descriptor of the size of the field in
034 * meters with "width" and "length" values. This is to account for arbitrary field sizes when
035 * transforming the poses.
036 *
037 * <p>Pose3ds in the JSON are measured using the normal FRC coordinate system, NWU with the origin
038 * at the bottom-right corner of the blue alliance wall. {@link #setOrigin(OriginPosition)} can be
039 * used to change the poses returned from {@link AprilTagFieldLayout#getTagPose(int)} to be from the
040 * perspective of a specific alliance.
041 *
042 * <p>Tag poses represent the center of the tag, with a zero rotation representing a tag that is
043 * upright and facing away from the (blue) alliance wall (that is, towards the opposing alliance).
044 */
045@JsonIgnoreProperties(ignoreUnknown = true)
046@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
047public class AprilTagFieldLayout {
048  /** Common origin positions for the AprilTag coordinate system. */
049  public enum OriginPosition {
050    /** Blue alliance wall, right side. */
051    kBlueAllianceWallRightSide,
052    /** Red alliance wall, right side. */
053    kRedAllianceWallRightSide,
054  }
055
056  private final Map<Integer, AprilTag> m_apriltags = new HashMap<>();
057
058  @JsonProperty(value = "field")
059  private FieldDimensions m_fieldDimensions;
060
061  private Pose3d m_origin;
062
063  /**
064   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
065   *
066   * @param path Path of the JSON file to import from.
067   * @throws IOException If reading from the file fails.
068   */
069  public AprilTagFieldLayout(String path) throws IOException {
070    this(Path.of(path));
071  }
072
073  /**
074   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
075   *
076   * @param path Path of the JSON file to import from.
077   * @throws IOException If reading from the file fails.
078   */
079  public AprilTagFieldLayout(Path path) throws IOException {
080    AprilTagFieldLayout layout =
081        new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class);
082    m_apriltags.putAll(layout.m_apriltags);
083    m_fieldDimensions = layout.m_fieldDimensions;
084    setOrigin(OriginPosition.kBlueAllianceWallRightSide);
085  }
086
087  /**
088   * Construct a new AprilTagFieldLayout from a list of {@link AprilTag} objects.
089   *
090   * @param apriltags List of {@link AprilTag}.
091   * @param fieldLength Length of the field the layout is representing in meters.
092   * @param fieldWidth Width of the field the layout is representing in meters.
093   */
094  public AprilTagFieldLayout(List<AprilTag> apriltags, double fieldLength, double fieldWidth) {
095    this(apriltags, new FieldDimensions(fieldLength, fieldWidth));
096  }
097
098  @JsonCreator
099  private AprilTagFieldLayout(
100      @JsonProperty(required = true, value = "tags") List<AprilTag> apriltags,
101      @JsonProperty(required = true, value = "field") FieldDimensions fieldDimensions) {
102    // To ensure the underlying semantics don't change with what kind of list is passed in
103    for (AprilTag tag : apriltags) {
104      m_apriltags.put(tag.ID, tag);
105    }
106    m_fieldDimensions = fieldDimensions;
107    setOrigin(OriginPosition.kBlueAllianceWallRightSide);
108  }
109
110  /**
111   * Returns a List of the {@link AprilTag AprilTags} used in this layout.
112   *
113   * @return The {@link AprilTag AprilTags} used in this layout.
114   */
115  @JsonProperty("tags")
116  public List<AprilTag> getTags() {
117    return new ArrayList<>(m_apriltags.values());
118  }
119
120  /**
121   * Returns the length of the field the layout is representing in meters.
122   *
123   * @return length, in meters
124   */
125  @JsonIgnore
126  public double getFieldLength() {
127    return m_fieldDimensions.fieldLength;
128  }
129
130  /**
131   * Returns the length of the field the layout is representing in meters.
132   *
133   * @return width, in meters
134   */
135  @JsonIgnore
136  public double getFieldWidth() {
137    return m_fieldDimensions.fieldWidth;
138  }
139
140  /**
141   * Sets the origin based on a predefined enumeration of coordinate frame origins. The origins are
142   * calculated from the field dimensions.
143   *
144   * <p>This transforms the Pose3d objects returned by {@link #getTagPose(int)} to return the
145   * correct pose relative to a predefined coordinate frame.
146   *
147   * @param origin The predefined origin
148   */
149  @JsonIgnore
150  public final void setOrigin(OriginPosition origin) {
151    switch (origin) {
152      case kBlueAllianceWallRightSide:
153        setOrigin(new Pose3d());
154        break;
155      case kRedAllianceWallRightSide:
156        setOrigin(
157            new Pose3d(
158                new Translation3d(m_fieldDimensions.fieldLength, m_fieldDimensions.fieldWidth, 0),
159                new Rotation3d(0, 0, Math.PI)));
160        break;
161      default:
162        throw new IllegalArgumentException("Unsupported enum value");
163    }
164  }
165
166  /**
167   * Sets the origin for tag pose transformation.
168   *
169   * <p>This transforms the Pose3d objects returned by {@link #getTagPose(int)} to return the
170   * correct pose relative to the provided origin.
171   *
172   * @param origin The new origin for tag transformations
173   */
174  @JsonIgnore
175  public final void setOrigin(Pose3d origin) {
176    m_origin = origin;
177  }
178
179  /**
180   * Returns the origin used for tag pose transformation.
181   *
182   * @return the origin
183   */
184  @JsonIgnore
185  public Pose3d getOrigin() {
186    return m_origin;
187  }
188
189  /**
190   * Gets an AprilTag pose by its ID.
191   *
192   * @param ID The ID of the tag.
193   * @return The pose corresponding to the ID passed in or an empty optional if a tag with that ID
194   *     was not found.
195   */
196  @SuppressWarnings("ParameterName")
197  public Optional<Pose3d> getTagPose(int ID) {
198    AprilTag tag = m_apriltags.get(ID);
199    if (tag == null) {
200      return Optional.empty();
201    }
202    return Optional.of(tag.pose.relativeTo(m_origin));
203  }
204
205  /**
206   * Serializes a AprilTagFieldLayout to a JSON file.
207   *
208   * @param path The path to write to.
209   * @throws IOException If writing to the file fails.
210   */
211  public void serialize(String path) throws IOException {
212    serialize(Path.of(path));
213  }
214
215  /**
216   * Serializes a AprilTagFieldLayout to a JSON file.
217   *
218   * @param path The path to write to.
219   * @throws IOException If writing to the file fails.
220   */
221  public void serialize(Path path) throws IOException {
222    new ObjectMapper().writeValue(path.toFile(), this);
223  }
224
225  /**
226   * Get an official {@link AprilTagFieldLayout}.
227   *
228   * @param field The loadable AprilTag field layout.
229   * @return AprilTagFieldLayout of the field.
230   * @throws UncheckedIOException If the layout does not exist.
231   */
232  public static AprilTagFieldLayout loadField(AprilTagFields field) {
233    try {
234      return loadFromResource(field.m_resourceFile);
235    } catch (IOException e) {
236      throw new UncheckedIOException(
237          "Could not load AprilTagFieldLayout from " + field.m_resourceFile, e);
238    }
239  }
240
241  /**
242   * Deserializes a field layout from a resource within a internal jar file.
243   *
244   * <p>Users should use {@link AprilTagFields#loadAprilTagLayoutField()} to load official layouts
245   * and {@link #AprilTagFieldLayout(String)} for custom layouts.
246   *
247   * @param resourcePath The absolute path of the resource
248   * @return The deserialized layout
249   * @throws IOException If the resource could not be loaded
250   */
251  public static AprilTagFieldLayout loadFromResource(String resourcePath) throws IOException {
252    InputStream stream = AprilTagFieldLayout.class.getResourceAsStream(resourcePath);
253    if (stream == null) {
254      // Class.getResourceAsStream() returns null if the resource does not exist.
255      throw new IOException("Could not locate resource: " + resourcePath);
256    }
257    InputStreamReader reader = new InputStreamReader(stream);
258    try {
259      return new ObjectMapper().readerFor(AprilTagFieldLayout.class).readValue(reader);
260    } catch (IOException e) {
261      throw new IOException("Failed to load AprilTagFieldLayout: " + resourcePath);
262    }
263  }
264
265  @Override
266  public boolean equals(Object obj) {
267    if (obj instanceof AprilTagFieldLayout) {
268      var other = (AprilTagFieldLayout) obj;
269      return m_apriltags.equals(other.m_apriltags) && m_origin.equals(other.m_origin);
270    }
271    return false;
272  }
273
274  @Override
275  public int hashCode() {
276    return Objects.hash(m_apriltags, m_origin);
277  }
278
279  @JsonIgnoreProperties(ignoreUnknown = true)
280  @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
281  private static class FieldDimensions {
282    @SuppressWarnings("MemberName")
283    @JsonProperty(value = "length")
284    public double fieldLength;
285
286    @SuppressWarnings("MemberName")
287    @JsonProperty(value = "width")
288    public double fieldWidth;
289
290    @JsonCreator()
291    FieldDimensions(
292        @JsonProperty(required = true, value = "length") double fieldLength,
293        @JsonProperty(required = true, value = "width") double fieldWidth) {
294      this.fieldLength = fieldLength;
295      this.fieldWidth = fieldWidth;
296    }
297  }
298}