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   * Returns the length of the field the layout is representing in meters.
118   *
119   * @return length, in meters
120   */
121  @JsonIgnore
122  public double getFieldLength() {
123    return m_fieldDimensions.fieldLength;
124  }
125
126  /**
127   * Returns the length of the field the layout is representing in meters.
128   *
129   * @return width, in meters
130   */
131  @JsonIgnore
132  public double getFieldWidth() {
133    return m_fieldDimensions.fieldWidth;
134  }
135
136  /**
137   * Sets the origin based on a predefined enumeration of coordinate frame origins. The origins are
138   * calculated from the field dimensions.
139   *
140   * <p>This transforms the Pose3d objects returned by {@link #getTagPose(int)} to return the
141   * correct pose relative to a predefined coordinate frame.
142   *
143   * @param origin The predefined origin
144   */
145  @JsonIgnore
146  public final void setOrigin(OriginPosition origin) {
147    switch (origin) {
148      case kBlueAllianceWallRightSide:
149        setOrigin(new Pose3d());
150        break;
151      case kRedAllianceWallRightSide:
152        setOrigin(
153            new Pose3d(
154                new Translation3d(m_fieldDimensions.fieldLength, m_fieldDimensions.fieldWidth, 0),
155                new Rotation3d(0, 0, Math.PI)));
156        break;
157      default:
158        throw new IllegalArgumentException("Unsupported enum value");
159    }
160  }
161
162  /**
163   * Sets the origin for tag pose transformation.
164   *
165   * <p>This transforms the Pose3d objects returned by {@link #getTagPose(int)} to return the
166   * correct pose relative to the provided origin.
167   *
168   * @param origin The new origin for tag transformations
169   */
170  @JsonIgnore
171  public final void setOrigin(Pose3d origin) {
172    m_origin = origin;
173  }
174
175  /**
176   * Returns the origin used for tag pose transformation.
177   *
178   * @return the origin
179   */
180  @JsonIgnore
181  public Pose3d getOrigin() {
182    return m_origin;
183  }
184
185  /**
186   * Gets an AprilTag pose by its ID.
187   *
188   * @param ID The ID of the tag.
189   * @return The pose corresponding to the ID passed in or an empty optional if a tag with that ID
190   *     was not found.
191   */
192  @SuppressWarnings("ParameterName")
193  public Optional<Pose3d> getTagPose(int ID) {
194    AprilTag tag = m_apriltags.get(ID);
195    if (tag == null) {
196      return Optional.empty();
197    }
198    return Optional.of(tag.pose.relativeTo(m_origin));
199  }
200
201  /**
202   * Serializes a AprilTagFieldLayout to a JSON file.
203   *
204   * @param path The path to write to.
205   * @throws IOException If writing to the file fails.
206   */
207  public void serialize(String path) throws IOException {
208    serialize(Path.of(path));
209  }
210
211  /**
212   * Serializes a AprilTagFieldLayout to a JSON file.
213   *
214   * @param path The path to write to.
215   * @throws IOException If writing to the file fails.
216   */
217  public void serialize(Path path) throws IOException {
218    new ObjectMapper().writeValue(path.toFile(), this);
219  }
220
221  /**
222   * Deserializes a field layout from a resource within a internal jar file.
223   *
224   * <p>Users should use {@link AprilTagFields#loadAprilTagLayoutField()} to load official layouts
225   * and {@link #AprilTagFieldLayout(String)} for custom layouts.
226   *
227   * @param resourcePath The absolute path of the resource
228   * @return The deserialized layout
229   * @throws IOException If the resource could not be loaded
230   */
231  public static AprilTagFieldLayout loadFromResource(String resourcePath) throws IOException {
232    InputStream stream = AprilTagFieldLayout.class.getResourceAsStream(resourcePath);
233    if (stream == null) {
234      // Class.getResourceAsStream() returns null if the resource does not exist.
235      throw new IOException("Could not locate resource: " + resourcePath);
236    }
237    InputStreamReader reader = new InputStreamReader(stream);
238    try {
239      return new ObjectMapper().readerFor(AprilTagFieldLayout.class).readValue(reader);
240    } catch (IOException e) {
241      throw new IOException("Failed to load AprilTagFieldLayout: " + resourcePath);
242    }
243  }
244
245  @Override
246  public boolean equals(Object obj) {
247    if (obj instanceof AprilTagFieldLayout) {
248      var other = (AprilTagFieldLayout) obj;
249      return m_apriltags.equals(other.m_apriltags) && m_origin.equals(other.m_origin);
250    }
251    return false;
252  }
253
254  @Override
255  public int hashCode() {
256    return Objects.hash(m_apriltags, m_origin);
257  }
258
259  @JsonIgnoreProperties(ignoreUnknown = true)
260  @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
261  private static class FieldDimensions {
262    @SuppressWarnings("MemberName")
263    @JsonProperty(value = "length")
264    public double fieldLength;
265
266    @SuppressWarnings("MemberName")
267    @JsonProperty(value = "width")
268    public double fieldWidth;
269
270    @JsonCreator()
271    FieldDimensions(
272        @JsonProperty(required = true, value = "length") double fieldLength,
273        @JsonProperty(required = true, value = "width") double fieldWidth) {
274      this.fieldLength = fieldLength;
275      this.fieldWidth = fieldWidth;
276    }
277  }
278}