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}