[wpimath] Add protobuf/struct for trivial types (#5935)

This implements de/serialization for the types that aren't templated (SwerveDriveKinematics) in C++ or where there is no trivial way to go round-trip (Splines) for the messages.

wpimath/src/test/java/edu/wpi/first/math/controller/proto/ArmFeedforwardProtoTest.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.ArmFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufArmFeedforward;
+import org.junit.jupiter.api.Test;
+
+class ArmFeedforwardProtoTest {
+  private static final ArmFeedforward DATA = new ArmFeedforward(0.174, 0.229, 4.4, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufArmFeedforward proto = ArmFeedforward.proto.createMessage();
+    ArmFeedforward.proto.pack(proto, DATA);
+
+    ArmFeedforward data = ArmFeedforward.proto.unpack(proto);
+    assertEquals(DATA.ks, data.ks);
+    assertEquals(DATA.kg, data.kg);
+    assertEquals(DATA.kv, data.kv);
+    assertEquals(DATA.ka, data.ka);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/proto/DifferentialDriveWheelVoltagesProtoTest.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.DifferentialDriveWheelVoltages;
+import edu.wpi.first.math.proto.Controller.ProtobufDifferentialDriveWheelVoltages;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelVoltagesProtoTest {
+  private static final DifferentialDriveWheelVoltages DATA =
+      new DifferentialDriveWheelVoltages(0.174, 0.191);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufDifferentialDriveWheelVoltages proto =
+        DifferentialDriveWheelVoltages.proto.createMessage();
+    DifferentialDriveWheelVoltages.proto.pack(proto, DATA);
+
+    DifferentialDriveWheelVoltages data = DifferentialDriveWheelVoltages.proto.unpack(proto);
+    assertEquals(DATA.left, data.left);
+    assertEquals(DATA.right, data.right);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/proto/ElevatorFeedforwardProtoTest.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.ElevatorFeedforward;
+import edu.wpi.first.math.proto.Controller.ProtobufElevatorFeedforward;
+import org.junit.jupiter.api.Test;
+
+class ElevatorFeedforwardProtoTest {
+  private static final ElevatorFeedforward DATA = new ElevatorFeedforward(1.91, 1.1, 1.1, 0.229);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufElevatorFeedforward proto = ElevatorFeedforward.proto.createMessage();
+    ElevatorFeedforward.proto.pack(proto, DATA);
+
+    ElevatorFeedforward data = ElevatorFeedforward.proto.unpack(proto);
+    assertEquals(DATA.ks, data.ks);
+    assertEquals(DATA.kg, data.kg);
+    assertEquals(DATA.kv, data.kv);
+    assertEquals(DATA.ka, data.ka);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/struct/ArmFeedforwardStructTest.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.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class ArmFeedforwardStructTest {
+  private static final ArmFeedforward DATA = new ArmFeedforward(0.174, 0.229, 4.4, 4.4);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(ArmFeedforward.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    ArmFeedforward.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    ArmFeedforward data = ArmFeedforward.struct.unpack(buffer);
+    assertEquals(DATA.ks, data.ks);
+    assertEquals(DATA.kg, data.kg);
+    assertEquals(DATA.kv, data.kv);
+    assertEquals(DATA.ka, data.ka);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/struct/DifferentialDriveWheelVoltagesStructTest.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.controller.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelVoltagesStructTest {
+  private static final DifferentialDriveWheelVoltages DATA =
+      new DifferentialDriveWheelVoltages(0.174, 0.191);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelVoltages.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    DifferentialDriveWheelVoltages.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    DifferentialDriveWheelVoltages data = DifferentialDriveWheelVoltages.struct.unpack(buffer);
+    assertEquals(DATA.left, data.left);
+    assertEquals(DATA.right, data.right);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/controller/struct/ElevatorFeedforwardStructTest.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.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class ElevatorFeedforwardStructTest {
+  private static final ElevatorFeedforward DATA = new ElevatorFeedforward(1.91, 1.1, 1.1, 0.229);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(ElevatorFeedforward.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    ElevatorFeedforward.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    ElevatorFeedforward data = ElevatorFeedforward.struct.unpack(buffer);
+    assertEquals(DATA.ks, data.ks);
+    assertEquals(DATA.kg, data.kg);
+    assertEquals(DATA.kv, data.kv);
+    assertEquals(DATA.ka, data.ka);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.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.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.proto.Kinematics.ProtobufChassisSpeeds;
+import org.junit.jupiter.api.Test;
+
+class ChassisSpeedsProtoTest {
+  private static final ChassisSpeeds DATA = new ChassisSpeeds(2.29, 2.2, 0.3504);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufChassisSpeeds proto = ChassisSpeeds.proto.createMessage();
+    ChassisSpeeds.proto.pack(proto, DATA);
+
+    ChassisSpeeds data = ChassisSpeeds.proto.unpack(proto);
+    assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond);
+    assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond);
+    assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.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.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveKinematics;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveKinematicsProtoTest {
+  private static final DifferentialDriveKinematics DATA = new DifferentialDriveKinematics(1.74);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufDifferentialDriveKinematics proto = DifferentialDriveKinematics.proto.createMessage();
+    DifferentialDriveKinematics.proto.pack(proto, DATA);
+
+    DifferentialDriveKinematics data = DifferentialDriveKinematics.proto.unpack(proto);
+    assertEquals(DATA.trackWidthMeters, data.trackWidthMeters);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.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.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelSpeeds;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelSpeedsProtoTest {
+  private static final DifferentialDriveWheelSpeeds DATA =
+      new DifferentialDriveWheelSpeeds(1.74, 35.04);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufDifferentialDriveWheelSpeeds proto = DifferentialDriveWheelSpeeds.proto.createMessage();
+    DifferentialDriveWheelSpeeds.proto.pack(proto, DATA);
+
+    DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.proto.unpack(proto);
+    assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond);
+    assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveKinematicsProtoTest.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.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveKinematics;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveKinematicsProtoTest {
+  private static final MecanumDriveKinematics DATA =
+      new MecanumDriveKinematics(
+          new Translation2d(19.1, 2.2),
+          new Translation2d(35.04, 1.91),
+          new Translation2d(1.74, 3.504),
+          new Translation2d(3.504, 1.91));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufMecanumDriveKinematics proto = MecanumDriveKinematics.proto.createMessage();
+    MecanumDriveKinematics.proto.pack(proto, DATA);
+
+    MecanumDriveKinematics data = MecanumDriveKinematics.proto.unpack(proto);
+    assertEquals(DATA.getFrontLeft(), data.getFrontLeft());
+    assertEquals(DATA.getFrontRight(), data.getFrontRight());
+    assertEquals(DATA.getRearLeft(), data.getRearLeft());
+    assertEquals(DATA.getRearRight(), data.getRearRight());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.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.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelPositions;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveWheelPositionsProtoTest {
+  private static final MecanumDriveWheelPositions DATA =
+      new MecanumDriveWheelPositions(17.4, 2.29, 22.9, 1.74);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufMecanumDriveWheelPositions proto = MecanumDriveWheelPositions.proto.createMessage();
+    MecanumDriveWheelPositions.proto.pack(proto, DATA);
+
+    MecanumDriveWheelPositions data = MecanumDriveWheelPositions.proto.unpack(proto);
+    assertEquals(DATA.frontLeftMeters, data.frontLeftMeters);
+    assertEquals(DATA.frontRightMeters, data.frontRightMeters);
+    assertEquals(DATA.rearLeftMeters, data.rearLeftMeters);
+    assertEquals(DATA.rearRightMeters, data.rearRightMeters);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.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.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveWheelSpeeds;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveWheelSpeedsProtoTest {
+  private static final MecanumDriveWheelSpeeds DATA =
+      new MecanumDriveWheelSpeeds(2.29, 17.4, 4.4, 0.229);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufMecanumDriveWheelSpeeds proto = MecanumDriveWheelSpeeds.proto.createMessage();
+    MecanumDriveWheelSpeeds.proto.pack(proto, DATA);
+
+    MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.proto.unpack(proto);
+    assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond);
+    assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond);
+    assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond);
+    assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.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.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModulePosition;
+import org.junit.jupiter.api.Test;
+
+class SwerveModulePositionProtoTest {
+  private static final SwerveModulePosition DATA =
+      new SwerveModulePosition(3.504, new Rotation2d(17.4));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufSwerveModulePosition proto = SwerveModulePosition.proto.createMessage();
+    SwerveModulePosition.proto.pack(proto, DATA);
+
+    SwerveModulePosition data = SwerveModulePosition.proto.unpack(proto);
+    assertEquals(DATA.distanceMeters, data.distanceMeters);
+    assertEquals(DATA.angle, data.angle);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.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.kinematics.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveModuleState;
+import org.junit.jupiter.api.Test;
+
+class SwerveModuleStateProtoTest {
+  private static final SwerveModuleState DATA = new SwerveModuleState(22.9, new Rotation2d(3.3));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufSwerveModuleState proto = SwerveModuleState.proto.createMessage();
+    SwerveModuleState.proto.pack(proto, DATA);
+
+    SwerveModuleState data = SwerveModuleState.proto.unpack(proto);
+    assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond);
+    assertEquals(DATA.angle, data.angle);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.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.assertEquals;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class ChassisSpeedsStructTest {
+  private static final ChassisSpeeds DATA = new ChassisSpeeds(2.29, 2.2, 0.3504);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(ChassisSpeeds.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    ChassisSpeeds.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    ChassisSpeeds data = ChassisSpeeds.struct.unpack(buffer);
+    assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond);
+    assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond);
+    assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.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.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveKinematicsStructTest {
+  private static final DifferentialDriveKinematics DATA = new DifferentialDriveKinematics(1.74);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveKinematics.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    DifferentialDriveKinematics.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    DifferentialDriveKinematics data = DifferentialDriveKinematics.struct.unpack(buffer);
+    assertEquals(DATA.trackWidthMeters, data.trackWidthMeters);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.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.assertEquals;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveWheelSpeedsStructTest {
+  private static final DifferentialDriveWheelSpeeds DATA =
+      new DifferentialDriveWheelSpeeds(1.74, 35.04);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelSpeeds.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    DifferentialDriveWheelSpeeds.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.struct.unpack(buffer);
+    assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond);
+    assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveKinematicsStructTest.java

@@ -0,0 +1,36 @@
+// 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.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveKinematicsStructTest {
+  private static final MecanumDriveKinematics DATA =
+      new MecanumDriveKinematics(
+          new Translation2d(19.1, 2.2),
+          new Translation2d(35.04, 1.91),
+          new Translation2d(1.74, 3.504),
+          new Translation2d(3.504, 1.91));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveKinematics.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    MecanumDriveKinematics.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    MecanumDriveKinematics data = MecanumDriveKinematics.struct.unpack(buffer);
+    assertEquals(DATA.getFrontLeft(), data.getFrontLeft());
+    assertEquals(DATA.getFrontRight(), data.getFrontRight());
+    assertEquals(DATA.getRearLeft(), data.getRearLeft());
+    assertEquals(DATA.getRearRight(), data.getRearRight());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.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.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveWheelPositionsStructTest {
+  private static final MecanumDriveWheelPositions DATA =
+      new MecanumDriveWheelPositions(17.4, 2.29, 22.9, 1.74);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveWheelPositions.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    MecanumDriveWheelPositions.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    MecanumDriveWheelPositions data = MecanumDriveWheelPositions.struct.unpack(buffer);
+    assertEquals(DATA.frontLeftMeters, data.frontLeftMeters);
+    assertEquals(DATA.frontRightMeters, data.frontRightMeters);
+    assertEquals(DATA.rearLeftMeters, data.rearLeftMeters);
+    assertEquals(DATA.rearRightMeters, data.rearRightMeters);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.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.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveWheelSpeedsStructTest {
+  private static final MecanumDriveWheelSpeeds DATA =
+      new MecanumDriveWheelSpeeds(2.29, 17.4, 4.4, 0.229);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveWheelSpeeds.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    MecanumDriveWheelSpeeds.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.struct.unpack(buffer);
+    assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond);
+    assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond);
+    assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond);
+    assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.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.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class SwerveModulePositionStructTest {
+  private static final SwerveModulePosition DATA =
+      new SwerveModulePosition(3.504, new Rotation2d(17.4));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(SwerveModulePosition.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    SwerveModulePosition.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    SwerveModulePosition data = SwerveModulePosition.struct.unpack(buffer);
+    assertEquals(DATA.distanceMeters, data.distanceMeters);
+    assertEquals(DATA.angle, data.angle);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.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.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class SwerveModuleStateStructTest {
+  private static final SwerveModuleState DATA = new SwerveModuleState(22.9, new Rotation2d(3.3));
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(SwerveModuleState.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    SwerveModuleState.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    SwerveModuleState data = SwerveModuleState.struct.unpack(buffer);
+    assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond);
+    assertEquals(DATA.angle, data.angle);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.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.system.plant.proto;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.proto.Plant.ProtobufDCMotor;
+import edu.wpi.first.math.system.plant.DCMotor;
+import org.junit.jupiter.api.Test;
+
+class DCMotorProtoTest {
+  private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufDCMotor proto = DCMotor.proto.createMessage();
+    DCMotor.proto.pack(proto, DATA);
+
+    DCMotor data = DCMotor.proto.unpack(proto);
+    assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts);
+    assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters);
+    assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps);
+    assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps);
+    assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec);
+    assertEquals(DATA.rOhms, data.rOhms);
+    assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt);
+    assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.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.system.plant.struct;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import org.junit.jupiter.api.Test;
+
+class DCMotorStructTest {
+  private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3);
+
+  @Test
+  void testRoundtrip() {
+    ByteBuffer buffer = ByteBuffer.allocate(DCMotor.struct.getSize());
+    buffer.order(ByteOrder.LITTLE_ENDIAN);
+    DCMotor.struct.pack(buffer, DATA);
+    buffer.rewind();
+
+    DCMotor data = DCMotor.struct.unpack(buffer);
+    assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts);
+    assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters);
+    assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps);
+    assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps);
+    assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec);
+    assertEquals(DATA.rOhms, data.rOhms);
+    assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt);
+    assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp);
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryProtoTest.java

@@ -0,0 +1,40 @@
+// 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.trajectory.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.Trajectory.ProtobufTrajectory;
+import edu.wpi.first.math.trajectory.Trajectory;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class TrajectoryProtoTest {
+  private static final Trajectory DATA =
+      new Trajectory(
+          List.of(
+              new Trajectory.State(
+                  1.1, 2.2, 3.3, new Pose2d(new Translation2d(1.1, 2.2), new Rotation2d(2.2)), 6.6),
+              new Trajectory.State(
+                  2.1, 2.2, 3.3, new Pose2d(new Translation2d(2.1, 2.2), new Rotation2d(2.2)), 6.6),
+              new Trajectory.State(
+                  3.1,
+                  2.2,
+                  3.3,
+                  new Pose2d(new Translation2d(3.1, 2.2), new Rotation2d(2.2)),
+                  6.6)));
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTrajectory proto = Trajectory.proto.createMessage();
+    Trajectory.proto.pack(proto, DATA);
+
+    Trajectory data = Trajectory.proto.unpack(proto);
+    assertEquals(DATA.getStates(), data.getStates());
+  }
+}

wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.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.trajectory.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.Trajectory.ProtobufTrajectoryState;
+import edu.wpi.first.math.trajectory.Trajectory;
+import org.junit.jupiter.api.Test;
+
+class TrajectoryStateProtoTest {
+  private static final Trajectory.State DATA =
+      new Trajectory.State(
+          1.91, 4.4, 17.4, new Pose2d(new Translation2d(1.74, 19.1), new Rotation2d(22.9)), 0.174);
+
+  @Test
+  void testRoundtrip() {
+    ProtobufTrajectoryState proto = Trajectory.State.proto.createMessage();
+    Trajectory.State.proto.pack(proto, DATA);
+
+    Trajectory.State data = Trajectory.State.proto.unpack(proto);
+    assertEquals(DATA.timeSeconds, data.timeSeconds);
+    assertEquals(DATA.velocityMetersPerSecond, data.velocityMetersPerSecond);
+    assertEquals(DATA.accelerationMetersPerSecondSq, data.accelerationMetersPerSecondSq);
+    assertEquals(DATA.poseMeters, data.poseMeters);
+    assertEquals(DATA.curvatureRadPerMeter, data.curvatureRadPerMeter);
+  }
+}