[wpimath] Add remaining struct and protobuf implementations (#5953)

wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveFeedforwardProtoTest.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.controller.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.DifferentialDriveFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveFeedforward;
+import edu.wpi.first.wpilibj.ProtoTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class DifferentialDriveFeedforwardProtoTest
+    extends ProtoTestBase<DifferentialDriveFeedforward, ProtobufDifferentialDriveFeedforward> {
+  DifferentialDriveFeedforwardProtoTest() {
+    super(
+        new DifferentialDriveFeedforward(0.174, 0.229, 4.4, 4.5),
+        DifferentialDriveFeedforward.proto);
+  }
+
+  @Override
+  public void checkEquals(
+      DifferentialDriveFeedforward testData, DifferentialDriveFeedforward data) {
+    assertEquals(testData.m_kVLinear, data.m_kVLinear);
+    assertEquals(testData.m_kALinear, data.m_kALinear);
+    assertEquals(testData.m_kVAngular, data.m_kVAngular);
+    assertEquals(testData.m_kAAngular, data.m_kAAngular);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/proto/SimpleMotorFeedforwardProtoTest.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.controller.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufSimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.ProtoTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class SimpleMotorFeedforwardProtoTest
+    extends ProtoTestBase<SimpleMotorFeedforward, ProtobufSimpleMotorFeedforward> {
+  SimpleMotorFeedforwardProtoTest() {
+    super(new SimpleMotorFeedforward(0.4, 4.0, 0.7, 0.025), SimpleMotorFeedforward.proto);
+  }
+
+  @Override
+  public void checkEquals(SimpleMotorFeedforward testData, SimpleMotorFeedforward data) {
+    assertEquals(testData.getKs(), data.getKs());
+    assertEquals(testData.getKv(), data.getKv());
+    assertEquals(testData.getKa(), data.getKa());
+    assertEquals(testData.getDt(), data.getDt());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveFeedforwardStructTest.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.controller.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.DifferentialDriveFeedforward;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class DifferentialDriveFeedforwardStructTest extends StructTestBase<DifferentialDriveFeedforward> {
+  DifferentialDriveFeedforwardStructTest() {
+    super(
+        new DifferentialDriveFeedforward(0.174, 0.229, 4.4, 4.5),
+        DifferentialDriveFeedforward.struct);
+  }
+
+  @Override
+  public void checkEquals(
+      DifferentialDriveFeedforward testData, DifferentialDriveFeedforward data) {
+    assertEquals(testData.m_kVLinear, data.m_kVLinear);
+    assertEquals(testData.m_kALinear, data.m_kALinear);
+    assertEquals(testData.m_kVAngular, data.m_kVAngular);
+    assertEquals(testData.m_kAAngular, data.m_kAAngular);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/struct/SimpleMotorFeedforwardStructTest.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.controller.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class SimpleMotorFeedforwardStructTest extends StructTestBase<SimpleMotorFeedforward> {
+  SimpleMotorFeedforwardStructTest() {
+    super(new SimpleMotorFeedforward(0.4, 4.0, 0.7, 0.025), SimpleMotorFeedforward.struct);
+  }
+
+  @Override
+  public void checkEquals(SimpleMotorFeedforward testData, SimpleMotorFeedforward data) {
+    assertEquals(testData.getKs(), data.getKs());
+    assertEquals(testData.getKv(), data.getKv());
+    assertEquals(testData.getKa(), data.getKa());
+    assertEquals(testData.getDt(), data.getDt());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProtoTest.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.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveMotorVoltages;
+import edu.wpi.first.wpilibj.ProtoTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class MecanumDriveMotorVoltagesProtoTest
+    extends ProtoTestBase<MecanumDriveMotorVoltages, ProtobufMecanumDriveMotorVoltages> {
+  MecanumDriveMotorVoltagesProtoTest() {
+    super(new MecanumDriveMotorVoltages(1.2, 3.1, 2.5, -0.1), MecanumDriveMotorVoltages.proto);
+  }
+
+  @Override
+  public void checkEquals(MecanumDriveMotorVoltages testData, MecanumDriveMotorVoltages data) {
+    assertEquals(testData.frontLeftVoltage, data.frontLeftVoltage);
+    assertEquals(testData.frontRightVoltage, data.frontRightVoltage);
+    assertEquals(testData.rearLeftVoltage, data.rearLeftVoltage);
+    assertEquals(testData.rearRightVoltage, data.rearRightVoltage);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveDriveKinematicsProtoTest.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.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveDriveKinematics;
+import edu.wpi.first.wpilibj.ProtoTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class SwerveDriveKinematicsProtoTest
+    extends ProtoTestBase<SwerveDriveKinematics, ProtobufSwerveDriveKinematics> {
+  SwerveDriveKinematicsProtoTest() {
+    super(
+        new SwerveDriveKinematics(
+            new Translation2d(1.0, 2.1),
+            new Translation2d(1.5, -0.9),
+            new Translation2d(-1.8, 1.2),
+            new Translation2d(-1.7, -1.3)),
+        SwerveDriveKinematics.proto);
+  }
+
+  @Override
+  public void checkEquals(SwerveDriveKinematics testData, SwerveDriveKinematics data) {
+    assertArrayEquals(testData.getModules(), data.getModules());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStructTest.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.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class MecanumDriveMotorVoltagesStructTest extends StructTestBase<MecanumDriveMotorVoltages> {
+  MecanumDriveMotorVoltagesStructTest() {
+    super(new MecanumDriveMotorVoltages(1.2, 3.1, 2.5, -0.1), MecanumDriveMotorVoltages.struct);
+  }
+
+  @Override
+  public void checkEquals(MecanumDriveMotorVoltages testData, MecanumDriveMotorVoltages data) {
+    assertEquals(testData.frontLeftVoltage, data.frontLeftVoltage);
+    assertEquals(testData.frontRightVoltage, data.frontRightVoltage);
+    assertEquals(testData.rearLeftVoltage, data.rearLeftVoltage);
+    assertEquals(testData.rearRightVoltage, data.rearRightVoltage);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveDriveKinematicsStructTest.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.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class SwerveDriveKinematicsStructTest extends StructTestBase<SwerveDriveKinematics> {
+  SwerveDriveKinematicsStructTest() {
+    super(
+        new SwerveDriveKinematics(
+            new Translation2d(1.0, 2.1),
+            new Translation2d(1.5, -0.9),
+            new Translation2d(-1.8, 1.2),
+            new Translation2d(-1.7, -1.3)),
+        SwerveDriveKinematics.getStruct(4));
+  }
+
+  @Override
+  public void checkEquals(SwerveDriveKinematics testData, SwerveDriveKinematics data) {
+    assertArrayEquals(testData.getModules(), data.getModules());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/proto/MatrixProtoTest.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.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.proto.Wpimath.ProtobufMatrix;
+import edu.wpi.first.wpilibj.ProtoTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class MatrixProtoTest extends ProtoTestBase<Matrix<N2, N3>, ProtobufMatrix> {
+  MatrixProtoTest() {
+    super(
+        MatBuilder.fill(Nat.N2(), Nat.N3(), 1.1, 1.2, 1.3, 1.4, 1.5, 1.6),
+        Matrix.getProto(Nat.N2(), Nat.N3()));
+  }
+
+  @Override
+  public void checkEquals(Matrix<N2, N3> testData, Matrix<N2, N3> data) {
+    assertEquals(testData, data);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/proto/VectorProtoTest.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.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.proto.Wpimath.ProtobufVector;
+import edu.wpi.first.wpilibj.ProtoTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class VectorProtoTest extends ProtoTestBase<Vector<N2>, ProtobufVector> {
+  VectorProtoTest() {
+    super(VecBuilder.fill(1.1, 1.2), Vector.getProto(Nat.N2()));
+  }
+
+  @Override
+  public void checkEquals(Vector<N2> testData, Vector<N2> data) {
+    assertEquals(testData, data);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/spline/proto/CubicHermiteSplineProtoTest.java

@@ -0,0 +1,33 @@
+// 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.spline.proto;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+
+import edu.wpi.first.math.proto.Spline.ProtobufCubicHermiteSpline;
+import edu.wpi.first.math.spline.CubicHermiteSpline;
+import edu.wpi.first.wpilibj.ProtoTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class CubicHermiteSplineProtoTest
+    extends ProtoTestBase<CubicHermiteSpline, ProtobufCubicHermiteSpline> {
+  CubicHermiteSplineProtoTest() {
+    super(
+        new CubicHermiteSpline(
+            new double[] {0.1, 0.3},
+            new double[] {0.4, -0.2},
+            new double[] {1.5, 1.3},
+            new double[] {-2.4, -1.1}),
+        CubicHermiteSpline.proto);
+  }
+
+  @Override
+  public void checkEquals(CubicHermiteSpline testData, CubicHermiteSpline data) {
+    assertArrayEquals(testData.xInitialControlVector, data.xInitialControlVector);
+    assertArrayEquals(testData.xFinalControlVector, data.xFinalControlVector);
+    assertArrayEquals(testData.yInitialControlVector, data.yInitialControlVector);
+    assertArrayEquals(testData.yFinalControlVector, data.yFinalControlVector);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/spline/proto/QuinticHermiteSplineProtoTest.java

@@ -0,0 +1,33 @@
+// 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.spline.proto;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+
+import edu.wpi.first.math.proto.Spline.ProtobufQuinticHermiteSpline;
+import edu.wpi.first.math.spline.QuinticHermiteSpline;
+import edu.wpi.first.wpilibj.ProtoTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class QuinticHermiteSplineProtoTest
+    extends ProtoTestBase<QuinticHermiteSpline, ProtobufQuinticHermiteSpline> {
+  QuinticHermiteSplineProtoTest() {
+    super(
+        new QuinticHermiteSpline(
+            new double[] {0.1, 0.3, 0.7},
+            new double[] {0.4, -0.2, -0.6},
+            new double[] {1.5, 1.3, 1.6},
+            new double[] {-2.4, -1.1, -2.1}),
+        QuinticHermiteSpline.proto);
+  }
+
+  @Override
+  public void checkEquals(QuinticHermiteSpline testData, QuinticHermiteSpline data) {
+    assertArrayEquals(testData.xInitialControlVector, data.xInitialControlVector);
+    assertArrayEquals(testData.xFinalControlVector, data.xFinalControlVector);
+    assertArrayEquals(testData.yInitialControlVector, data.yInitialControlVector);
+    assertArrayEquals(testData.yFinalControlVector, data.yFinalControlVector);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/spline/struct/CubicHermiteSplineStructTest.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.spline.struct;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+
+import edu.wpi.first.math.spline.CubicHermiteSpline;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class CubicHermiteSplineStructTest extends StructTestBase<CubicHermiteSpline> {
+  CubicHermiteSplineStructTest() {
+    super(
+        new CubicHermiteSpline(
+            new double[] {0.1, 0.3},
+            new double[] {0.4, -0.2},
+            new double[] {1.5, 1.3},
+            new double[] {-2.4, -1.1}),
+        CubicHermiteSpline.struct);
+  }
+
+  @Override
+  public void checkEquals(CubicHermiteSpline testData, CubicHermiteSpline data) {
+    assertArrayEquals(testData.xInitialControlVector, data.xInitialControlVector);
+    assertArrayEquals(testData.xFinalControlVector, data.xFinalControlVector);
+    assertArrayEquals(testData.yInitialControlVector, data.yInitialControlVector);
+    assertArrayEquals(testData.yFinalControlVector, data.yFinalControlVector);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/spline/struct/QuinticHermiteSplineStructTest.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.spline.struct;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+
+import edu.wpi.first.math.spline.QuinticHermiteSpline;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class QuinticHermiteSplineStructTest extends StructTestBase<QuinticHermiteSpline> {
+  QuinticHermiteSplineStructTest() {
+    super(
+        new QuinticHermiteSpline(
+            new double[] {0.1, 0.3, 0.7},
+            new double[] {0.4, -0.2, -0.6},
+            new double[] {1.5, 1.3, 1.6},
+            new double[] {-2.4, -1.1, -2.1}),
+        QuinticHermiteSpline.struct);
+  }
+
+  @Override
+  public void checkEquals(QuinticHermiteSpline testData, QuinticHermiteSpline data) {
+    assertArrayEquals(testData.xInitialControlVector, data.xInitialControlVector);
+    assertArrayEquals(testData.xFinalControlVector, data.xFinalControlVector);
+    assertArrayEquals(testData.yInitialControlVector, data.yInitialControlVector);
+    assertArrayEquals(testData.yFinalControlVector, data.yFinalControlVector);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/struct/MatrixStructTest.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.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class MatrixStructTest extends StructTestBase<Matrix<N2, N3>> {
+  MatrixStructTest() {
+    super(
+        MatBuilder.fill(Nat.N2(), Nat.N3(), 1.1, 1.2, 1.3, 1.4, 1.5, 1.6),
+        Matrix.getStruct(Nat.N2(), Nat.N3()));
+  }
+
+  @Override
+  public void checkEquals(Matrix<N2, N3> testData, Matrix<N2, N3> data) {
+    assertEquals(testData, data);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/struct/VectorStructTest.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.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class VectorStructTest extends StructTestBase<Vector<N2>> {
+  VectorStructTest() {
+    super(VecBuilder.fill(1.1, 1.2), Vector.getStruct(Nat.N2()));
+  }
+
+  @Override
+  public void checkEquals(Vector<N2> testData, Vector<N2> data) {
+    assertEquals(testData, data);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/system/proto/LinearSystemProtoTest.java

@@ -0,0 +1,39 @@
+// 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.system.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.math.proto.System.ProtobufLinearSystem;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.wpilibj.ProtoTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class LinearSystemProtoTest extends ProtoTestBase<LinearSystem<N2, N3, N4>, ProtobufLinearSystem> {
+  LinearSystemProtoTest() {
+    super(
+        new LinearSystem<>(
+            MatBuilder.fill(Nat.N2(), Nat.N2(), 1.1, 1.2, 1.3, 1.4),
+            MatBuilder.fill(Nat.N2(), Nat.N3(), 2.1, 2.2, 2.3, 2.4, 2.5, 2.6),
+            MatBuilder.fill(Nat.N4(), Nat.N2(), 3.1, 3.2, 3.3, 3.4, 3.5, 3.6, 3.7, 3.8),
+            MatBuilder.fill(
+                Nat.N4(), Nat.N3(), 4.01, 4.02, 4.03, 4.04, 4.05, 4.06, 4.07, 4.08, 4.09, 4.10,
+                4.11, 4.12)),
+        LinearSystem.getProto(Nat.N2(), Nat.N3(), Nat.N4()));
+  }
+
+  @Override
+  public void checkEquals(LinearSystem<N2, N3, N4> testData, LinearSystem<N2, N3, N4> data) {
+    assertEquals(testData.getA(), data.getA());
+    assertEquals(testData.getB(), data.getB());
+    assertEquals(testData.getC(), data.getC());
+    assertEquals(testData.getD(), data.getD());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/system/struct/LinearSystemStructTest.java

@@ -0,0 +1,38 @@
+// 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.system.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class LinearSystemStructTest extends StructTestBase<LinearSystem<N2, N3, N4>> {
+  LinearSystemStructTest() {
+    super(
+        new LinearSystem<>(
+            MatBuilder.fill(Nat.N2(), Nat.N2(), 1.1, 1.2, 1.3, 1.4),
+            MatBuilder.fill(Nat.N2(), Nat.N3(), 2.1, 2.2, 2.3, 2.4, 2.5, 2.6),
+            MatBuilder.fill(Nat.N4(), Nat.N2(), 3.1, 3.2, 3.3, 3.4, 3.5, 3.6, 3.7, 3.8),
+            MatBuilder.fill(
+                Nat.N4(), Nat.N3(), 4.01, 4.02, 4.03, 4.04, 4.05, 4.06, 4.07, 4.08, 4.09, 4.10,
+                4.11, 4.12)),
+        LinearSystem.getStruct(Nat.N2(), Nat.N3(), Nat.N4()));
+  }
+
+  @Override
+  public void checkEquals(LinearSystem<N2, N3, N4> testData, LinearSystem<N2, N3, N4> data) {
+    assertEquals(testData.getA(), data.getA());
+    assertEquals(testData.getB(), data.getB());
+    assertEquals(testData.getC(), data.getC());
+    assertEquals(testData.getD(), data.getD());
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/ProtoTestBase.java

@@ -0,0 +1,52 @@
+// 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.wpilibj;
+
+import edu.wpi.first.util.protobuf.Protobuf;
+import org.junit.jupiter.api.Test;
+import us.hebi.quickbuf.ProtoMessage;
+
+public abstract class ProtoTestBase<T, MessageType extends ProtoMessage<MessageType>> {
+  private final T m_testData;
+  private final Protobuf<T, MessageType> m_proto;
+
+  protected ProtoTestBase(T testData, Protobuf<T, MessageType> proto) {
+    m_testData = testData;
+    m_proto = proto;
+  }
+
+  public abstract void checkEquals(T testData, T data);
+
+  @Test
+  void testRoundTrip() {
+    final MessageType msg = m_proto.createMessage();
+    m_proto.pack(msg, m_testData);
+
+    final T data = m_proto.unpack(msg);
+    checkEquals(m_testData, data);
+  }
+
+  @Test
+  void testDoublePack() {
+    final MessageType msg = m_proto.createMessage();
+    m_proto.pack(msg, m_testData);
+    m_proto.pack(msg, m_testData);
+
+    final T data = m_proto.unpack(msg);
+    checkEquals(m_testData, data);
+  }
+
+  @Test
+  void testDoubleUnpack() {
+    final MessageType msg = m_proto.createMessage();
+    m_proto.pack(msg, m_testData);
+
+    T data = m_proto.unpack(msg);
+    checkEquals(m_testData, data);
+
+    data = m_proto.unpack(msg);
+    checkEquals(m_testData, data);
+  }
+}

wpimath/src/test/java/edu/wpi/first/wpilibj/StructTestBase.java

@@ -0,0 +1,61 @@
+// 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.wpilibj;
+
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+public abstract class StructTestBase<T> {
+  private final T m_testData;
+  private final Struct<T> m_struct;
+
+  protected StructTestBase(T testData, Struct<T> struct) {
+    m_testData = testData;
+    m_struct = struct;
+  }
+
+  public abstract void checkEquals(T testData, T data);
+
+  @Test
+  void testRoundTrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(m_struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    m_struct.pack(buffer, m_testData);
+    buffer.rewind();
+
+    final T data = m_struct.unpack(buffer);
+    checkEquals(m_testData, data);
+  }
+
+  @Test
+  void testDoublePack() {
+    ByteBuffer buffer = ByteBuffer.allocate(m_struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    m_struct.pack(buffer, m_testData);
+    buffer.rewind();
+    m_struct.pack(buffer, m_testData);
+    buffer.rewind();
+
+    final T data = m_struct.unpack(buffer);
+    checkEquals(m_testData, data);
+  }
+
+  @Test
+  void testDoubleUnpack() {
+    ByteBuffer buffer = ByteBuffer.allocate(m_struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    m_struct.pack(buffer, m_testData);
+    buffer.rewind();
+
+    T data = m_struct.unpack(buffer);
+    checkEquals(m_testData, data);
+    buffer.rewind();
+
+    data = m_struct.unpack(buffer);
+    checkEquals(m_testData, data);
+  }
+}