Add struct implementations for SwerveDriveKinematics

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

@@ -13,6 +13,7 @@
 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;
@@ -423,4 +424,14 @@ public Translation2d[] getModules() {
 
   /** 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/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 getTypeString() {
+    return "struct: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);
+  }
+}

wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.h

@@ -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.
+
+#pragma once
+
+#include <fmt/format.h>
+#include <wpi/ct_string.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/kinematics/SwerveDriveKinematics.h"
+
+template <size_t NumModules>
+struct wpi::Struct<frc::SwerveDriveKinematics<NumModules>> {
+  static constexpr ct_string kTypeString =
+      wpi::Concat("struct:SwerveDriveKinematics__"_ct_string,
+                  wpi::NumToCtString<NumModules>());
+  static constexpr std::string_view GetTypeString() { return kTypeString; }
+  static constexpr size_t GetSize() {
+    return NumModules * wpi::Struct<frc::Translation2d>::GetSize();
+  }
+  static constexpr ct_string kSchema =
+      wpi::Concat("Translation2d modules["_ct_string,
+                  wpi::NumToCtString<NumModules>(), "]"_ct_string);
+  static constexpr std::string_view GetSchema() { return kSchema; }
+
+  static frc::SwerveDriveKinematics<NumModules> Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::SwerveDriveKinematics<NumModules>& value);
+};
+
+static_assert(wpi::StructSerializable<frc::SwerveDriveKinematics<4>>);
+static_assert(wpi::StructSerializable<frc::SwerveDriveKinematics<3>>);
+
+#include "frc/kinematics/struct/SwerveDriveKinematicsStruct.inc"

wpimath/src/main/native/include/frc/kinematics/struct/SwerveDriveKinematicsStruct.inc

@@ -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.
+
+#pragma once
+
+#include "frc/kinematics/struct/SwerveDriveKinematicsStruct.h"
+
+namespace {
+constexpr size_t kModulesOff = 0;
+}  // namespace
+
+template <size_t NumModules>
+frc::SwerveDriveKinematics<NumModules>
+wpi::Struct<frc::SwerveDriveKinematics<NumModules>>::Unpack(
+    std::span<const uint8_t> data) {
+  return frc::SwerveDriveKinematics<NumModules>{
+      wpi::UnpackStructArray<frc::Translation2d, kModulesOff, NumModules>(
+          data)};
+}
+
+template <size_t NumModules>
+void wpi::Struct<frc::SwerveDriveKinematics<NumModules>>::Pack(
+    std::span<uint8_t> data,
+    const frc::SwerveDriveKinematics<NumModules>& value) {
+  wpi::PackStructArray<kModulesOff, NumModules>(data, value.GetModules());
+}

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

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics.struct;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.StructTestBase;
+
+@SuppressWarnings("PMD.TestClassWithoutTestCases")
+class SwerveDriveKinematicsStructTest extends StructTestBase<SwerveDriveKinematics> {
+  SwerveDriveKinematicsStructTest() {
+    super(
+        new SwerveDriveKinematics(
+            new Translation2d(1.0, 2.1),
+            new Translation2d(1.5, -0.9),
+            new Translation2d(-1.8, 1.2),
+            new Translation2d(-1.7, -1.3)),
+        SwerveDriveKinematics.getStruct(4));
+  }
+
+  @Override
+  public void checkEquals(SwerveDriveKinematics testData, SwerveDriveKinematics data) {
+    assertArrayEquals(testData.getModules(), data.getModules());
+  }
+}

wpimath/src/test/native/cpp/kinematics/struct/SwerveDriveKinematicsStructTest.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 "../../StructTestBase.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/kinematics/struct/SwerveDriveKinematicsStruct.h"
+
+using namespace frc;
+
+struct SwerveDriveKinematicsStructTestData {
+  using Type = SwerveDriveKinematics<4>;
+
+  inline static const Type kTestData{
+      frc::Translation2d{1.0_m, 0.9_m}, frc::Translation2d{1.1_m, -0.8_m},
+      frc::Translation2d{-1.2_m, 0.7_m}, frc::Translation2d{-1.3_m, -0.6_m}};
+
+  static void CheckEq(const Type& testData, const Type& data) {
+    EXPECT_EQ(testData.GetModules(), data.GetModules());
+  }
+};
+
+INSTANTIATE_TYPED_TEST_SUITE_P(SwerveDriveKinematics, StructTest,
+                               SwerveDriveKinematicsStructTestData);

wpiutil/src/main/java/edu/wpi/first/util/struct/Struct.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.util.struct;
 
+import java.lang.reflect.Array;
 import java.nio.ByteBuffer;
 
 /**
@@ -114,6 +115,26 @@ default void unpackInto(T out, ByteBuffer bb) {
     throw new UnsupportedOperationException("object does not support unpackInto");
   }
 
+  /**
+   * Deserializes an array from a raw struct serialized ByteBuffer starting at the current
+   * position. Will increment the ByteBuffer position by size * struct.size() bytes. Will not
+   * otherwise modify the ByteBuffer (e.g. byte order will not be changed).
+   *
+   * @param bb ByteBuffer
+   * @param size Size of the array
+   * @param struct Struct implementation
+   * @return Deserialized array
+   */
+  static <T> T[] unpackArray(ByteBuffer bb, int size, Struct<T> struct) {
+    @SuppressWarnings("unchecked")
+    T[] arr = (T[]) Array.newInstance(struct.getTypeClass(), size);
+    for (int i = 0; i < arr.length; i++) {
+      arr[i] = struct.unpack(bb);
+    }
+    return arr;
+  }
+
+
   /**
    * Deserializes a double array from a raw struct serialized ByteBuffer starting at the current
    * position. Will increment the ByteBuffer position by size * kSizeDouble bytes. Will not
@@ -131,6 +152,21 @@ static double[] unpackDoubleArray(ByteBuffer bb, int size) {
     return arr;
   }
 
+  /**
+   * Puts array contents to a ByteBuffer starting at the current position. Will increment the
+   * ByteBuffer position by size * struct.size() bytes. Will not otherwise modify the ByteBuffer (e.g.
+   * byte order will not be changed).
+   *
+   * @param bb ByteBuffer
+   * @param arr Array to serialize
+   * @param struct Struct implementation
+   */
+  static <T> void packArray(ByteBuffer bb, T[] arr, Struct<T> struct) {
+    for (T obj : arr) {
+      struct.pack(bb, obj);
+    }
+  }
+
   /**
    * Puts array contents to a ByteBuffer starting at the current position. Will increment the
    * ByteBuffer position by size * kSizeDouble bytes. Will not otherwise modify the ByteBuffer (e.g.