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}