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}