[wpimath] Move struct/proto classes to separate files (#5918)

Also add unit tests.

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d;
+import org.junit.jupiter.api.Test;
+
+class Pose2dProtoTest {
+  private static final Pose2d DATA =
+      new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufPose2d proto = Pose2d.proto.createMessage();
+    Pose2d.proto.pack(proto, DATA);
+
+    Pose2d data = Pose2d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d;
+import org.junit.jupiter.api.Test;
+
+class Pose3dProtoTest {
+  private static final Pose3d DATA =
+      new Pose3d(
+          new Translation3d(1.1, 2.2, 1.1),
+          new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74)));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufPose3d proto = Pose3d.proto.createMessage();
+    Pose3d.proto.pack(proto, DATA);
+
+    Pose3d data = Pose3d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion;
+import org.junit.jupiter.api.Test;
+
+class QuaternionProtoTest {
+  private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufQuaternion proto = Quaternion.proto.createMessage();
+    Quaternion.proto.pack(proto, DATA);
+
+    Quaternion data = Quaternion.proto.unpack(proto);
+    assertEquals(DATA.getW(), data.getW());
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation2dProtoTest.java

@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d;
+import org.junit.jupiter.api.Test;
+
+class Rotation2dProtoTest {
+  private static final Rotation2d DATA = new Rotation2d(1.91);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufRotation2d proto = Rotation2d.proto.createMessage();
+    Rotation2d.proto.pack(proto, DATA);
+
+    Rotation2d data = Rotation2d.proto.unpack(proto);
+    assertEquals(DATA.getRadians(), data.getRadians());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation3dProtoTest.java

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d;
+import org.junit.jupiter.api.Test;
+
+class Rotation3dProtoTest {
+  private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufRotation3d proto = Rotation3d.proto.createMessage();
+    Rotation3d.proto.pack(proto, DATA);
+
+    Rotation3d data = Rotation3d.proto.unpack(proto);
+    assertEquals(DATA.getQuaternion(), data.getQuaternion());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform2dProtoTest.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d;
+import org.junit.jupiter.api.Test;
+
+class Transform2dProtoTest {
+  private static final Transform2d DATA =
+      new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTransform2d proto = Transform2d.proto.createMessage();
+    Transform2d.proto.pack(proto, DATA);
+
+    Transform2d data = Transform2d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform3dProtoTest.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d;
+import org.junit.jupiter.api.Test;
+
+class Transform3dProtoTest {
+  private static final Transform3d DATA =
+      new Transform3d(
+          new Translation3d(0.3504, 22.9, 3.504),
+          new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504)));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTransform3d proto = Transform3d.proto.createMessage();
+    Transform3d.proto.pack(proto, DATA);
+
+    Transform3d data = Transform3d.proto.unpack(proto);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation2dProtoTest.java

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d;
+import org.junit.jupiter.api.Test;
+
+class Translation2dProtoTest {
+  private static final Translation2d DATA = new Translation2d(3.504, 22.9);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTranslation2d proto = Translation2d.proto.createMessage();
+    Translation2d.proto.pack(proto, DATA);
+
+    Translation2d data = Translation2d.proto.unpack(proto);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation3dProtoTest.java

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d;
+import org.junit.jupiter.api.Test;
+
+class Translation3dProtoTest {
+  private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTranslation3d proto = Translation3d.proto.createMessage();
+    Translation3d.proto.pack(proto, DATA);
+
+    Translation3d data = Translation3d.proto.unpack(proto);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist2dProtoTest.java

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d;
+import org.junit.jupiter.api.Test;
+
+class Twist2dProtoTest {
+  private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTwist2d proto = Twist2d.proto.createMessage();
+    Twist2d.proto.pack(proto, DATA);
+
+    Twist2d data = Twist2d.proto.unpack(proto);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dtheta, data.dtheta);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist3dProtoTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist3d;
+import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d;
+import org.junit.jupiter.api.Test;
+
+class Twist3dProtoTest {
+  private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTwist3d proto = Twist3d.proto.createMessage();
+    Twist3d.proto.pack(proto, DATA);
+
+    Twist3d data = Twist3d.proto.unpack(proto);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dz, data.dz);
+    assertEquals(DATA.rx, data.rx);
+    assertEquals(DATA.ry, data.ry);
+    assertEquals(DATA.rz, data.rz);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose2dStructTest.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Pose2dStructTest {
+  private static final Pose2d DATA =
+      new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Pose2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Pose2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Pose2d data = Pose2d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose3dStructTest.java

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Pose3dStructTest {
+  private static final Pose3d DATA =
+      new Pose3d(
+          new Translation3d(1.1, 2.2, 1.1),
+          new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74)));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Pose3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Pose3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Pose3d data = Pose3d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/QuaternionStructTest.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class QuaternionStructTest {
+  private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Quaternion.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Quaternion.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Quaternion data = Quaternion.struct.unpack(buffer);
+    assertEquals(DATA.getW(), data.getW());
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation2dStructTest.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Rotation2dStructTest {
+  private static final Rotation2d DATA = new Rotation2d(1.91);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Rotation2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Rotation2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Rotation2d data = Rotation2d.struct.unpack(buffer);
+    assertEquals(DATA.getRadians(), data.getRadians());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation3dStructTest.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Rotation3dStructTest {
+  private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Rotation3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Rotation3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Rotation3d data = Rotation3d.struct.unpack(buffer);
+    assertEquals(DATA.getQuaternion(), data.getQuaternion());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform2dStructTest.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Transform2dStructTest {
+  private static final Transform2d DATA =
+      new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Transform2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Transform2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Transform2d data = Transform2d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform3dStructTest.java

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Transform3dStructTest {
+  private static final Transform3d DATA =
+      new Transform3d(
+          new Translation3d(0.3504, 22.9, 3.504),
+          new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504)));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Transform3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Transform3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Transform3d data = Transform3d.struct.unpack(buffer);
+    assertEquals(DATA.getTranslation(), data.getTranslation());
+    assertEquals(DATA.getRotation(), data.getRotation());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation2dStructTest.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Translation2dStructTest {
+  private static final Translation2d DATA = new Translation2d(3.504, 22.9);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Translation2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Translation2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Translation2d data = Translation2d.struct.unpack(buffer);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation3dStructTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Translation3dStructTest {
+  private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Translation3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Translation3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Translation3d data = Translation3d.struct.unpack(buffer);
+    assertEquals(DATA.getX(), data.getX());
+    assertEquals(DATA.getY(), data.getY());
+    assertEquals(DATA.getZ(), data.getZ());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist2dStructTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist2d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Twist2dStructTest {
+  private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Twist2d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Twist2d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Twist2d data = Twist2d.struct.unpack(buffer);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dtheta, data.dtheta);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist3dStructTest.java

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Twist3d;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class Twist3dStructTest {
+  private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(Twist3d.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    Twist3d.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    Twist3d data = Twist3d.struct.unpack(buffer);
+    assertEquals(DATA.dx, data.dx);
+    assertEquals(DATA.dy, data.dy);
+    assertEquals(DATA.dz, data.dz);
+    assertEquals(DATA.rx, data.rx);
+    assertEquals(DATA.ry, data.ry);
+    assertEquals(DATA.rz, data.rz);
+  }
+}

wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.cpp

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Pose2d>;
+
+const Pose2d kExpectedData =
+    Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}};
+}  // namespace
+
+TEST(Pose2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Pose2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.cpp

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Pose3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Pose3d>;
+
+const Pose3d kExpectedData =
+    Pose3d{Translation3d{1.1_m, 2.2_m, 1.1_m},
+           Rotation3d{Quaternion{1.91, 0.3504, 3.3, 1.74}}};
+}  // namespace
+
+TEST(Pose3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Pose3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.cpp

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Quaternion.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Quaternion>;
+
+const Quaternion kExpectedData = Quaternion{1.1, 0.191, 35.04, 19.1};
+}  // namespace
+
+TEST(QuaternionProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Quaternion unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.W(), unpacked_data.W());
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}

wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Rotation2d>;
+
+const Rotation2d kExpectedData = Rotation2d{1.91_rad};
+}  // namespace
+
+TEST(Rotation2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Rotation2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Radians().value(), unpacked_data.Radians().value());
+}

wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.cpp

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Rotation3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Rotation3d>;
+
+const Rotation3d kExpectedData =
+    Rotation3d{Quaternion{2.29, 0.191, 0.191, 17.4}};
+}  // namespace
+
+TEST(Rotation3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Rotation3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.GetQuaternion(), unpacked_data.GetQuaternion());
+}

wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.cpp

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Transform2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Transform2d>;
+
+const Transform2d kExpectedData =
+    Transform2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{4.4_rad}};
+}  // namespace
+
+TEST(Transform2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Transform2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.cpp

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Transform3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Transform3d>;
+
+const Transform3d kExpectedData =
+    Transform3d{Translation3d{0.3504_m, 22.9_m, 3.504_m},
+                Rotation3d{Quaternion{0.3504, 35.04, 2.29, 0.3504}}};
+}  // namespace
+
+TEST(Transform3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Transform3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.cpp

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Translation2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Translation2d>;
+
+const Translation2d kExpectedData = Translation2d{3.504_m, 22.9_m};
+}  // namespace
+
+TEST(Translation2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Translation2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.X().value(), unpacked_data.X().value());
+  EXPECT_EQ(kExpectedData.Y().value(), unpacked_data.Y().value());
+}

wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.cpp

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Translation3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Translation3d>;
+
+const Translation3d kExpectedData = Translation3d{35.04_m, 22.9_m, 3.504_m};
+}  // namespace
+
+TEST(Translation3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Translation3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}

wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.cpp

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Twist2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Twist2d>;
+
+const Twist2d kExpectedData = Twist2d{2.29_m, 35.04_m, 35.04_rad};
+}  // namespace
+
+TEST(Twist2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Twist2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dtheta.value(), unpacked_data.dtheta.value());
+}

wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Twist3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Twist3d>;
+
+const Twist3d kExpectedData =
+    Twist3d{1.1_m, 2.29_m, 35.04_m, 0.174_rad, 19.1_rad, 4.4_rad};
+}  // namespace
+
+TEST(Twist3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Twist3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dz.value(), unpacked_data.dz.value());
+  EXPECT_EQ(kExpectedData.rx.value(), unpacked_data.rx.value());
+  EXPECT_EQ(kExpectedData.ry.value(), unpacked_data.ry.value());
+  EXPECT_EQ(kExpectedData.rz.value(), unpacked_data.rz.value());
+}

wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Pose2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Pose2d>;
+const Pose2d kExpectedData{
+    Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}};
+}  // namespace
+
+TEST(Pose2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Pose2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Pose3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Pose3d>;
+const Pose3d kExpectedData{
+    Pose3d{Translation3d{1.1_m, 2.2_m, 1.1_m},
+           Rotation3d{Quaternion{1.91, 0.3504, 3.3, 1.74}}}};
+}  // namespace
+
+TEST(Pose3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Pose3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Quaternion.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Quaternion>;
+const Quaternion kExpectedData{Quaternion{1.1, 0.191, 35.04, 19.1}};
+}  // namespace
+
+TEST(QuaternionStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Quaternion unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.W(), unpacked_data.W());
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}

wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Rotation2d>;
+const Rotation2d kExpectedData{Rotation2d{1.91_rad}};
+}  // namespace
+
+TEST(Rotation2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Rotation2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Radians(), unpacked_data.Radians());
+}

wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Rotation3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Rotation3d>;
+const Rotation3d kExpectedData{
+    Rotation3d{Quaternion{2.29, 0.191, 0.191, 17.4}}};
+}  // namespace
+
+TEST(Rotation3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Rotation3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.GetQuaternion(), unpacked_data.GetQuaternion());
+}

wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Transform2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Transform2d>;
+const Transform2d kExpectedData{
+    Transform2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{4.4_rad}}};
+}  // namespace
+
+TEST(Transform2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Transform2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Transform3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Transform3d>;
+const Transform3d kExpectedData{
+    Transform3d{Translation3d{0.3504_m, 22.9_m, 3.504_m},
+                Rotation3d{Quaternion{0.3504, 35.04, 2.29, 0.3504}}}};
+}  // namespace
+
+TEST(Transform3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Transform3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Translation2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Translation2d>;
+const Translation2d kExpectedData{Translation2d{3.504_m, 22.9_m}};
+}  // namespace
+
+TEST(Translation2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Translation2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+}

wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Translation3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Translation3d>;
+const Translation3d kExpectedData{Translation3d{35.04_m, 22.9_m, 3.504_m}};
+}  // namespace
+
+TEST(Translation3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Translation3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}

wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Twist2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Twist2d>;
+const Twist2d kExpectedData{Twist2d{2.29_m, 35.04_m, 35.04_rad}};
+}  // namespace
+
+TEST(Twist2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Twist2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dtheta.value(), unpacked_data.dtheta.value());
+}

wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Twist3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Twist3d>;
+const Twist3d kExpectedData{
+    Twist3d{1.1_m, 2.29_m, 35.04_m, 0.174_rad, 19.1_rad, 4.4_rad}};
+}  // namespace
+
+TEST(Twist3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Twist3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dz.value(), unpacked_data.dz.value());
+  EXPECT_EQ(kExpectedData.rx.value(), unpacked_data.rx.value());
+  EXPECT_EQ(kExpectedData.ry.value(), unpacked_data.ry.value());
+  EXPECT_EQ(kExpectedData.rz.value(), unpacked_data.rz.value());
+}