Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add AprilTag and AprilTagFieldLayout (#4421)

Browse Source 
This is an API for looking up a Pose3d from a tag id, and includes functionality to load that map from a JSON file.

This also adds JSON support to Pose3d, Rotation3d. Translation3d, and Quaternion.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: AMereBagatelle <themerebagatelle@gmail.com>
This commit is contained in:
Brennen Puth
2022-11-04 12:56:22 -04:00
committed by GitHub
parent 7aab8fa93a
commit b2b473b24a
22 changed files with 813 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -4,6 +4,10 @@
package edu.wpi.first.math.geometry;
import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
@@ -14,6 +18,8 @@ import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
/** Represents a 3D pose containing translational and rotational elements. */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Pose3d implements Interpolatable<Pose3d> {
private final Translation3d m_translation;
private final Rotation3d m_rotation;
@@ -30,7 +36,10 @@ public class Pose3d implements Interpolatable<Pose3d> {
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
public Pose3d(Translation3d translation, Rotation3d rotation) {
@JsonCreator
public Pose3d(
@JsonProperty(required = true, value = "translation") Translation3d translation,
@JsonProperty(required = true, value = "rotation") Rotation3d rotation) {
m_translation = translation;
m_rotation = rotation;
}
@@ -84,6 +93,7 @@ public class Pose3d implements Interpolatable<Pose3d> {
*
* @return The translational component of the pose.
*/
@JsonProperty
public Translation3d getTranslation() {
return m_translation;
}
@@ -120,6 +130,7 @@ public class Pose3d implements Interpolatable<Pose3d> {
*
* @return The rotational component of the pose.
*/
@JsonProperty
public Rotation3d getRotation() {
return m_rotation;
}

17
wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
View File

@@ -4,11 +4,17 @@
package edu.wpi.first.math.geometry;
import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.numbers.N3;
import java.util.Objects;
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Quaternion {
private final double m_r;
private final Vector<N3> m_v;
@@ -27,7 +33,12 @@ public class Quaternion {
* @param y Y component of the quaternion.
* @param z Z component of the quaternion.
*/
public Quaternion(double w, double x, double y, double z) {
@JsonCreator
public Quaternion(
@JsonProperty(required = true, value = "W") double w,
@JsonProperty(required = true, value = "X") double x,
@JsonProperty(required = true, value = "Y") double y,
@JsonProperty(required = true, value = "Z") double z) {
m_r = w;
m_v = VecBuilder.fill(x, y, z);
}
@@ -113,6 +124,7 @@ public class Quaternion {
*
* @return W component of the quaternion.
*/
@JsonProperty(value = "W")
public double getW() {
return m_r;
}
@@ -122,6 +134,7 @@ public class Quaternion {
*
* @return X component of the quaternion.
*/
@JsonProperty(value = "X")
public double getX() {
return m_v.get(0, 0);
}
@@ -131,6 +144,7 @@ public class Quaternion {
*
* @return Y component of the quaternion.
*/
@JsonProperty(value = "Y")
public double getY() {
return m_v.get(1, 0);
}
@@ -140,6 +154,7 @@ public class Quaternion {
*
* @return Z component of the quaternion.
*/
@JsonProperty(value = "Z")
public double getZ() {
return m_v.get(2, 0);
}

10
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -4,6 +4,10 @@
package edu.wpi.first.math.geometry;
import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
@@ -17,6 +21,8 @@ import java.util.Objects;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
/** A rotation in a 3D coordinate frame represented by a quaternion. */
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Rotation3d implements Interpolatable<Rotation3d> {
private Quaternion m_q = new Quaternion();
@@ -28,7 +34,8 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
*
* @param q The quaternion.
*/
public Rotation3d(Quaternion q) {
@JsonCreator
public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) {
m_q = q.normalize();
}
@@ -270,6 +277,7 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
*
* @return The quaternion representation of the Rotation3d.
*/
@JsonProperty(value = "quaternion")
public Quaternion getQuaternion() {
return m_q;
}

15
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -4,6 +4,10 @@
package edu.wpi.first.math.geometry;
import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.interpolation.Interpolatable;
import java.util.Objects;
@@ -15,6 +19,8 @@ import java.util.Objects;
* origin facing in the positive X direction, forward is positive X, left is positive Y, and up is
* positive Z.
*/
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Translation3d implements Interpolatable<Translation3d> {
private final double m_x;
private final double m_y;
@@ -32,7 +38,11 @@ public class Translation3d implements Interpolatable<Translation3d> {
* @param y The y component of the translation.
* @param z The z component of the translation.
*/
public Translation3d(double x, double y, double z) {
@JsonCreator
public Translation3d(
@JsonProperty(required = true, value = "x") double x,
@JsonProperty(required = true, value = "y") double y,
@JsonProperty(required = true, value = "z") double z) {
m_x = x;
m_y = y;
m_z = z;
@@ -70,6 +80,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
*
* @return The X component of the translation.
*/
@JsonProperty
public double getX() {
return m_x;
}
@@ -79,6 +90,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
*
* @return The Y component of the translation.
*/
@JsonProperty
public double getY() {
return m_y;
}
@@ -88,6 +100,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
*
* @return The Z component of the translation.
*/
@JsonProperty
public double getZ() {
return m_z;
}