[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);
+  }
+}

wpimath/src/test/native/cpp/controller/proto/ArmFeedforwardProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "controller.pb.h"
+#include "frc/controller/ArmFeedforward.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::ArmFeedforward>;
+
+static constexpr auto Ks = 1.91_V;
+static constexpr auto Kg = 2.29_V;
+static constexpr auto Kv = 35.04_V * 1_s / 1_rad;
+static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_rad;
+const ArmFeedforward kExpectedData{Ks, Kg, Kv, Ka};
+}  // namespace
+
+TEST(ArmFeedforwardProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  ArmFeedforward unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value());
+  EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value());
+  EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value());
+  EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value());
+}

wpimath/src/test/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProtoTest.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 "controller.pb.h"
+#include "frc/controller/DifferentialDriveWheelVoltages.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::DifferentialDriveWheelVoltages>;
+
+const DifferentialDriveWheelVoltages kExpectedData =
+    DifferentialDriveWheelVoltages{0.174_V, 0.191_V};
+}  // namespace
+
+TEST(DifferentialDriveWheelVoltagesProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  DifferentialDriveWheelVoltages unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value());
+  EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value());
+}

wpimath/src/test/native/cpp/controller/proto/ElevatorFeedforwardProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "controller.pb.h"
+#include "frc/controller/ElevatorFeedforward.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::ElevatorFeedforward>;
+
+static constexpr auto Ks = 1.91_V;
+static constexpr auto Kg = 2.29_V;
+static constexpr auto Kv = 35.04_V * 1_s / 1_m;
+static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_m;
+
+constexpr ElevatorFeedforward kExpectedData{Ks, Kg, Kv, Ka};
+}  // namespace
+
+TEST(ElevatorFeedforwardProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  ElevatorFeedforward unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value());
+  EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value());
+  EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value());
+  EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value());
+}

wpimath/src/test/native/cpp/controller/struct/ArmFeedforwardStructTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/controller/ArmFeedforward.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::ArmFeedforward>;
+
+static constexpr auto Ks = 1.91_V;
+static constexpr auto Kg = 2.29_V;
+static constexpr auto Kv = 35.04_V * 1_s / 1_rad;
+static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_rad;
+const ArmFeedforward kExpectedData{Ks, Kg, Kv, Ka};
+}  // namespace
+
+TEST(ArmFeedforwardStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  ArmFeedforward unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value());
+  EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value());
+  EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value());
+  EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value());
+}

wpimath/src/test/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStructTest.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/controller/DifferentialDriveWheelVoltages.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::DifferentialDriveWheelVoltages>;
+const DifferentialDriveWheelVoltages kExpectedData{
+    DifferentialDriveWheelVoltages{0.174_V, 0.191_V}};
+}  // namespace
+
+TEST(DifferentialDriveWheelVoltagesStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  DifferentialDriveWheelVoltages unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value());
+  EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value());
+}

wpimath/src/test/native/cpp/controller/struct/ElevatorFeedforwardStructTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/controller/ElevatorFeedforward.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::ElevatorFeedforward>;
+
+static constexpr auto Ks = 1.91_V;
+static constexpr auto Kg = 2.29_V;
+static constexpr auto Kv = 35.04_V * 1_s / 1_m;
+static constexpr auto Ka = 1.74_V * 1_s * 1_s / 1_m;
+
+constexpr ElevatorFeedforward kExpectedData{Ks, Kg, Kv, Ka};
+}  // namespace
+
+TEST(ElevatorFeedforwardStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  ElevatorFeedforward unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.kS.value(), unpacked_data.kS.value());
+  EXPECT_EQ(kExpectedData.kG.value(), unpacked_data.kG.value());
+  EXPECT_EQ(kExpectedData.kV.value(), unpacked_data.kV.value());
+  EXPECT_EQ(kExpectedData.kA.value(), unpacked_data.kA.value());
+}

wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.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/kinematics/ChassisSpeeds.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::ChassisSpeeds>;
+
+const ChassisSpeeds kExpectedData =
+    ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s};
+}  // namespace
+
+TEST(ChassisSpeedsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  ChassisSpeeds unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.vx.value(), unpacked_data.vx.value());
+  EXPECT_EQ(kExpectedData.vy.value(), unpacked_data.vy.value());
+  EXPECT_EQ(kExpectedData.omega.value(), unpacked_data.omega.value());
+}

wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.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/kinematics/DifferentialDriveKinematics.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::DifferentialDriveKinematics>;
+
+const DifferentialDriveKinematics kExpectedData =
+    DifferentialDriveKinematics{1.74_m};
+}  // namespace
+
+TEST(DifferentialDriveKinematicsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  DifferentialDriveKinematics unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.trackWidth.value(), unpacked_data.trackWidth.value());
+}

wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.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/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::DifferentialDriveWheelSpeeds>;
+
+const DifferentialDriveWheelSpeeds kExpectedData =
+    DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps};
+}  // namespace
+
+TEST(DifferentialDriveWheelSpeedsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  DifferentialDriveWheelSpeeds unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value());
+  EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value());
+}

wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveKinematicsProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::MecanumDriveKinematics>;
+
+const MecanumDriveKinematics kExpectedData = MecanumDriveKinematics{
+    Translation2d{19.1_m, 2.2_m}, Translation2d{35.04_m, 1.91_m},
+    Translation2d{1.74_m, 3.504_m}, Translation2d{3.504_m, 1.91_m}};
+}  // namespace
+
+TEST(MecanumDriveKinematicsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  MecanumDriveKinematics unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.GetFrontLeftWheel(),
+            unpacked_data.GetFrontLeftWheel());
+  EXPECT_EQ(kExpectedData.GetFrontRightWheel(),
+            unpacked_data.GetFrontRightWheel());
+  EXPECT_EQ(kExpectedData.GetRearLeftWheel(), unpacked_data.GetRearLeftWheel());
+  EXPECT_EQ(kExpectedData.GetRearRightWheel(),
+            unpacked_data.GetRearRightWheel());
+}

wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::MecanumDriveWheelPositions>;
+
+const MecanumDriveWheelPositions kExpectedData =
+    MecanumDriveWheelPositions{17.4_m, 2.29_m, 22.9_m, 1.74_m};
+}  // namespace
+
+TEST(MecanumDriveWheelPositionsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  MecanumDriveWheelPositions unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value());
+  EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value());
+  EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value());
+  EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value());
+}

wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::MecanumDriveWheelSpeeds>;
+
+const MecanumDriveWheelSpeeds kExpectedData =
+    MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps};
+}  // namespace
+
+TEST(MecanumDriveWheelSpeedsProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  MecanumDriveWheelSpeeds unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value());
+  EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value());
+  EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value());
+  EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value());
+}

wpimath/src/test/native/cpp/kinematics/proto/SwerveModulePositionProtoTest.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/kinematics/SwerveModulePosition.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::SwerveModulePosition>;
+
+const SwerveModulePosition kExpectedData =
+    SwerveModulePosition{3.504_m, Rotation2d{17.4_rad}};
+}  // namespace
+
+TEST(SwerveModulePositionProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  SwerveModulePosition unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.distance.value(), unpacked_data.distance.value());
+  EXPECT_EQ(kExpectedData.angle, unpacked_data.angle);
+}

wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.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/kinematics/SwerveModuleState.h"
+#include "kinematics.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::SwerveModuleState>;
+
+const SwerveModuleState kExpectedData =
+    SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}};
+}  // namespace
+
+TEST(SwerveModuleStateProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  SwerveModuleState unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.speed.value(), unpacked_data.speed.value());
+  EXPECT_EQ(kExpectedData.angle, unpacked_data.angle);
+}

wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.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/kinematics/ChassisSpeeds.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::ChassisSpeeds>;
+const ChassisSpeeds kExpectedData{
+    ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s}};
+}  // namespace
+
+TEST(ChassisSpeedsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  ChassisSpeeds unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.vx.value(), unpacked_data.vx.value());
+  EXPECT_EQ(kExpectedData.vy.value(), unpacked_data.vy.value());
+  EXPECT_EQ(kExpectedData.omega.value(), unpacked_data.omega.value());
+}

wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.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/kinematics/DifferentialDriveKinematics.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::DifferentialDriveKinematics>;
+const DifferentialDriveKinematics kExpectedData{
+    DifferentialDriveKinematics{1.74_m}};
+}  // namespace
+
+TEST(DifferentialDriveKinematicsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  DifferentialDriveKinematics unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.trackWidth.value(), unpacked_data.trackWidth.value());
+}

wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.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/kinematics/DifferentialDriveWheelSpeeds.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::DifferentialDriveWheelSpeeds>;
+const DifferentialDriveWheelSpeeds kExpectedData{
+    DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps}};
+}  // namespace
+
+TEST(DifferentialDriveWheelSpeedsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  DifferentialDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value());
+  EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value());
+}

wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::MecanumDriveKinematics>;
+const MecanumDriveKinematics kExpectedData{MecanumDriveKinematics{
+    Translation2d{19.1_m, 2.2_m}, Translation2d{35.04_m, 1.91_m},
+    Translation2d{1.74_m, 3.504_m}, Translation2d{3.504_m, 1.91_m}}};
+}  // namespace
+
+TEST(MecanumDriveKinematicsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  MecanumDriveKinematics unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.GetFrontLeftWheel(),
+            unpacked_data.GetFrontLeftWheel());
+  EXPECT_EQ(kExpectedData.GetFrontRightWheel(),
+            unpacked_data.GetFrontRightWheel());
+  EXPECT_EQ(kExpectedData.GetRearLeftWheel(), unpacked_data.GetRearLeftWheel());
+  EXPECT_EQ(kExpectedData.GetRearRightWheel(),
+            unpacked_data.GetRearRightWheel());
+}

wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.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/kinematics/MecanumDriveWheelPositions.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::MecanumDriveWheelPositions>;
+const MecanumDriveWheelPositions kExpectedData{
+    MecanumDriveWheelPositions{17.4_m, 2.29_m, 22.9_m, 1.74_m}};
+}  // namespace
+
+TEST(MecanumDriveWheelPositionsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  MecanumDriveWheelPositions unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value());
+  EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value());
+  EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value());
+  EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value());
+}

wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.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/kinematics/MecanumDriveWheelSpeeds.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::MecanumDriveWheelSpeeds>;
+const MecanumDriveWheelSpeeds kExpectedData{
+    MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps}};
+}  // namespace
+
+TEST(MecanumDriveWheelSpeedsStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  MecanumDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value());
+  EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value());
+  EXPECT_EQ(kExpectedData.rearLeft.value(), unpacked_data.rearLeft.value());
+  EXPECT_EQ(kExpectedData.rearRight.value(), unpacked_data.rearRight.value());
+}

wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.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/kinematics/SwerveModulePosition.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::SwerveModulePosition>;
+const SwerveModulePosition kExpectedData{
+    SwerveModulePosition{3.504_m, Rotation2d{17.4_rad}}};
+}  // namespace
+
+TEST(SwerveModulePositionStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  SwerveModulePosition unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.distance.value(), unpacked_data.distance.value());
+  EXPECT_EQ(kExpectedData.angle, unpacked_data.angle);
+}

wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.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/kinematics/SwerveModuleState.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::SwerveModuleState>;
+const SwerveModuleState kExpectedData{
+    SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}}};
+}  // namespace
+
+TEST(SwerveModuleStateStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  SwerveModuleState unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.speed.value(), unpacked_data.speed.value());
+  EXPECT_EQ(kExpectedData.angle, unpacked_data.angle);
+}

wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp

@@ -0,0 +1,42 @@
+// 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/system/plant/DCMotor.h"
+#include "plant.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::DCMotor>;
+
+const DCMotor kExpectedData = DCMotor{units::volt_t{1.91},
+                                      units::newton_meter_t{19.1},
+                                      units::ampere_t{1.74},
+                                      units::ampere_t{2.29},
+                                      units::radians_per_second_t{2.2},
+                                      2};
+}  // namespace
+
+TEST(DCMotorProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  DCMotor unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.nominalVoltage.value(),
+            unpacked_data.nominalVoltage.value());
+  EXPECT_EQ(kExpectedData.stallTorque.value(),
+            unpacked_data.stallTorque.value());
+  EXPECT_EQ(kExpectedData.stallCurrent.value(),
+            unpacked_data.stallCurrent.value());
+  EXPECT_EQ(kExpectedData.freeCurrent.value(),
+            unpacked_data.freeCurrent.value());
+  EXPECT_EQ(kExpectedData.freeSpeed.value(), unpacked_data.freeSpeed.value());
+  EXPECT_EQ(kExpectedData.R.value(), unpacked_data.R.value());
+  EXPECT_EQ(kExpectedData.Kv.value(), unpacked_data.Kv.value());
+  EXPECT_EQ(kExpectedData.Kt.value(), unpacked_data.Kt.value());
+}

wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp

@@ -0,0 +1,41 @@
+// 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/system/plant/DCMotor.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::DCMotor>;
+const DCMotor kExpectedData = DCMotor{units::volt_t{1.91},
+                                      units::newton_meter_t{19.1},
+                                      units::ampere_t{1.74},
+                                      units::ampere_t{2.29},
+                                      units::radians_per_second_t{2.2},
+                                      2};
+}  // namespace
+
+TEST(DCMotorStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  DCMotor unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.nominalVoltage.value(),
+            unpacked_data.nominalVoltage.value());
+  EXPECT_EQ(kExpectedData.stallTorque.value(),
+            unpacked_data.stallTorque.value());
+  EXPECT_EQ(kExpectedData.stallCurrent.value(),
+            unpacked_data.stallCurrent.value());
+  EXPECT_EQ(kExpectedData.freeCurrent.value(),
+            unpacked_data.freeCurrent.value());
+  EXPECT_EQ(kExpectedData.freeSpeed.value(), unpacked_data.freeSpeed.value());
+  EXPECT_EQ(kExpectedData.R.value(), unpacked_data.R.value());
+  EXPECT_EQ(kExpectedData.Kv.value(), unpacked_data.Kv.value());
+  EXPECT_EQ(kExpectedData.Kt.value(), unpacked_data.Kt.value());
+}

wpimath/src/test/native/cpp/trajectory/proto/TrajectoryProtoTest.cpp

@@ -0,0 +1,35 @@
+// 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/trajectory/Trajectory.h"
+#include "trajectory.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Trajectory>;
+
+const Trajectory kExpectedData = Trajectory{std::vector<frc::Trajectory::State>{
+    Trajectory::State{1.1_s, 2.2_mps, 3.3_mps_sq,
+                      Pose2d(Translation2d(1.1_m, 2.2_m), Rotation2d(2.2_rad)),
+                      units::curvature_t{6.6}},
+    Trajectory::State{2.1_s, 2.2_mps, 3.3_mps_sq,
+                      Pose2d(Translation2d(2.1_m, 2.2_m), Rotation2d(2.2_rad)),
+                      units::curvature_t{6.6}},
+    Trajectory::State{3.1_s, 2.2_mps, 3.3_mps_sq,
+                      Pose2d(Translation2d(3.1_m, 2.2_m), Rotation2d(2.2_rad)),
+                      units::curvature_t{6.6}}}};
+}  // namespace
+
+TEST(TrajectoryProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Trajectory unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.States(), unpacked_data.States());
+}

wpimath/src/test/native/cpp/trajectory/proto/TrajectoryStateProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/trajectory/Trajectory.h"
+#include "trajectory.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Trajectory::State>;
+
+const Trajectory::State kExpectedData = Trajectory::State{
+    1.91_s, 4.4_mps, 17.4_mps_sq,
+    Pose2d{Translation2d{1.74_m, 19.1_m}, Rotation2d{22.9_rad}},
+    units::curvature_t{0.174}};
+}  // namespace
+
+TEST(TrajectoryStateProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Trajectory::State unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.t.value(), unpacked_data.t.value());
+  EXPECT_EQ(kExpectedData.velocity.value(), unpacked_data.velocity.value());
+  EXPECT_EQ(kExpectedData.acceleration.value(),
+            unpacked_data.acceleration.value());
+  EXPECT_EQ(kExpectedData.pose, unpacked_data.pose);
+  EXPECT_EQ(kExpectedData.curvature.value(), unpacked_data.curvature.value());
+}