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

wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java

@@ -4,8 +4,13 @@
 
 package edu.wpi.first.math.kinematics;
 
+import edu.wpi.first.math.kinematics.proto.MecanumDriveMotorVoltagesProto;
+import edu.wpi.first.math.kinematics.struct.MecanumDriveMotorVoltagesStruct;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.StructSerializable;
+
 /** Represents the motor voltages for a mecanum drive drivetrain. */
-public class MecanumDriveMotorVoltages {
+public class MecanumDriveMotorVoltages implements ProtobufSerializable, StructSerializable {
   /** Voltage of the front left motor. */
   public double frontLeftVoltage;
 
@@ -47,4 +52,11 @@ public class MecanumDriveMotorVoltages {
             + "Rear Left: %.2f V, Rear Right: %.2f V)",
         frontLeftVoltage, frontRightVoltage, rearLeftVoltage, rearRightVoltage);
   }
+
+  /** MecanumDriveMotorVoltages struct for serialization. */
+  public static final MecanumDriveMotorVoltagesStruct struct =
+      new MecanumDriveMotorVoltagesStruct();
+
+  /** MecanumDriveMotorVoltages protobuf for serialization. */
+  public static final MecanumDriveMotorVoltagesProto proto = new MecanumDriveMotorVoltagesProto();
 }

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java

@@ -12,10 +12,15 @@ import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.proto.SwerveDriveKinematicsProto;
+import edu.wpi.first.math.kinematics.struct.SwerveDriveKinematicsStruct;
 import edu.wpi.first.units.Angle;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Velocity;
+import edu.wpi.first.util.protobuf.ProtobufSerializable;
+import edu.wpi.first.util.struct.Struct;
+import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Arrays;
 import org.ejml.simple.SimpleMatrix;
 
@@ -41,7 +46,9 @@ import org.ejml.simple.SimpleMatrix;
  */
 @SuppressWarnings("overrides")
 public class SwerveDriveKinematics
-    implements Kinematics<SwerveModuleState[], SwerveModulePosition[]> {
+    implements Kinematics<SwerveModuleState[], SwerveModulePosition[]>,
+        ProtobufSerializable,
+        StructSerializable {
   private final SimpleMatrix m_inverseKinematics;
   private final SimpleMatrix m_forwardKinematics;
 
@@ -415,4 +422,28 @@ public class SwerveDriveKinematics
     }
     return newPositions;
   }
+
+  /**
+   * Gets the locations of the modules relative to the center of rotation.
+   *
+   * @return The locations of the modules relative to the center of rotation. This array should not
+   *     be modified.
+   */
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
+  public Translation2d[] getModules() {
+    return m_modules;
+  }
+
+  /** SwerveDriveKinematics protobuf for serialization. */
+  public static final SwerveDriveKinematicsProto proto = new SwerveDriveKinematicsProto();
+
+  /**
+   * Creates an implementation of the {@link Struct} interface for swerve drive kinematics objects.
+   *
+   * @param numModules The number of modules of the kinematics objects this serializer processes.
+   * @return The struct implementation.
+   */
+  public static final SwerveDriveKinematicsStruct getStruct(int numModules) {
+    return new SwerveDriveKinematicsStruct(numModules);
+  }
 }

wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveMotorVoltagesProto.java

@@ -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.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.math.proto.Kinematics.ProtobufMecanumDriveMotorVoltages;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public final class MecanumDriveMotorVoltagesProto
+    implements Protobuf<MecanumDriveMotorVoltages, ProtobufMecanumDriveMotorVoltages> {
+  @Override
+  public Class<MecanumDriveMotorVoltages> getTypeClass() {
+    return MecanumDriveMotorVoltages.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufMecanumDriveMotorVoltages.getDescriptor();
+  }
+
+  @Override
+  public ProtobufMecanumDriveMotorVoltages createMessage() {
+    return ProtobufMecanumDriveMotorVoltages.newInstance();
+  }
+
+  @Override
+  public MecanumDriveMotorVoltages unpack(ProtobufMecanumDriveMotorVoltages msg) {
+    return new MecanumDriveMotorVoltages(
+        msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight());
+  }
+
+  @Override
+  public void pack(ProtobufMecanumDriveMotorVoltages msg, MecanumDriveMotorVoltages value) {
+    msg.setFrontLeft(value.frontLeftVoltage);
+    msg.setFrontRight(value.frontRightVoltage);
+    msg.setRearLeft(value.rearLeftVoltage);
+    msg.setRearRight(value.rearRightVoltage);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveDriveKinematicsProto.java

@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics.proto;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.proto.Kinematics.ProtobufSwerveDriveKinematics;
+import edu.wpi.first.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public final class SwerveDriveKinematicsProto
+    implements Protobuf<SwerveDriveKinematics, ProtobufSwerveDriveKinematics> {
+  @Override
+  public Class<SwerveDriveKinematics> getTypeClass() {
+    return SwerveDriveKinematics.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufSwerveDriveKinematics.getDescriptor();
+  }
+
+  @Override
+  public ProtobufSwerveDriveKinematics createMessage() {
+    return ProtobufSwerveDriveKinematics.newInstance();
+  }
+
+  @Override
+  public SwerveDriveKinematics unpack(ProtobufSwerveDriveKinematics msg) {
+    return new SwerveDriveKinematics(Protobuf.unpackArray(msg.getModules(), Translation2d.proto));
+  }
+
+  @Override
+  public void pack(ProtobufSwerveDriveKinematics msg, SwerveDriveKinematics value) {
+    Protobuf.packArray(msg.getMutableModules(), value.getModules(), Translation2d.proto);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveMotorVoltagesStruct.java

@@ -0,0 +1,48 @@
+// 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 edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public final class MecanumDriveMotorVoltagesStruct implements Struct<MecanumDriveMotorVoltages> {
+  @Override
+  public Class<MecanumDriveMotorVoltages> getTypeClass() {
+    return MecanumDriveMotorVoltages.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "MecanumDriveMotorVoltages";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double front_left;double front_right;double rear_left;double rear_right";
+  }
+
+  @Override
+  public MecanumDriveMotorVoltages unpack(ByteBuffer bb) {
+    double front_left = bb.getDouble();
+    double front_right = bb.getDouble();
+    double rear_left = bb.getDouble();
+    double rear_right = bb.getDouble();
+    return new MecanumDriveMotorVoltages(front_left, front_right, rear_left, rear_right);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, MecanumDriveMotorVoltages value) {
+    bb.putDouble(value.frontLeftVoltage);
+    bb.putDouble(value.frontRightVoltage);
+    bb.putDouble(value.rearLeftVoltage);
+    bb.putDouble(value.rearRightVoltage);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveDriveKinematicsStruct.java

@@ -0,0 +1,58 @@
+// 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 edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.util.struct.Struct;
+import java.nio.ByteBuffer;
+
+public final class SwerveDriveKinematicsStruct implements Struct<SwerveDriveKinematics> {
+  private final int m_numModules;
+
+  /**
+   * Constructs the {@link Struct} implementation.
+   *
+   * @param numModules the number of modules of the kinematics objects this serializer processes.
+   */
+  public SwerveDriveKinematicsStruct(int numModules) {
+    m_numModules = numModules;
+  }
+
+  @Override
+  public Class<SwerveDriveKinematics> getTypeClass() {
+    return SwerveDriveKinematics.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "SwerveDriveKinematics__" + m_numModules;
+  }
+
+  @Override
+  public int getSize() {
+    return Translation2d.struct.getSize() * m_numModules;
+  }
+
+  @Override
+  public String getSchema() {
+    return "Translation2d modules[" + m_numModules + "]";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Translation2d.struct};
+  }
+
+  @Override
+  public SwerveDriveKinematics unpack(ByteBuffer bb) {
+    return new SwerveDriveKinematics(Struct.unpackArray(bb, m_numModules, Translation2d.struct));
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, SwerveDriveKinematics value) {
+    Struct.packArray(bb, value.getModules(), Translation2d.struct);
+  }
+}