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

wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h

@@ -299,10 +299,14 @@ class SwerveDriveKinematics
     return {result};
   }
 
+  const wpi::array<Translation2d, NumModules> GetModules() const {
+    return m_modules;
+  }
+
  private:
+  wpi::array<Translation2d, NumModules> m_modules;
   mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
-  wpi::array<Translation2d, NumModules> m_modules;
   mutable wpi::array<Rotation2d, NumModules> m_moduleHeadings;
 
   mutable Translation2d m_previousCoR;

wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.h

@@ -0,0 +1,20 @@
+// 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 <wpi/protobuf/Protobuf.h>
+
+#include "frc/kinematics/SwerveDriveKinematics.h"
+
+template <size_t NumModules>
+struct wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::SwerveDriveKinematics<NumModules> Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::SwerveDriveKinematics<NumModules>& value);
+};
+
+#include "frc/kinematics/proto/SwerveDriveKinematicsProto.inc"

wpimath/src/main/native/include/frc/kinematics/proto/SwerveDriveKinematicsProto.inc

@@ -0,0 +1,44 @@
+// 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 <stdexcept>
+
+#include <fmt/format.h>
+#include <wpi/ProtoHelper.h>
+
+#include "frc/kinematics/proto/SwerveDriveKinematicsProto.h"
+#include "kinematics.pb.h"
+
+template <size_t NumModules>
+google::protobuf::Message*
+wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>>::New(
+    google::protobuf::Arena* arena) {
+  return wpi::CreateMessage<wpi::proto::ProtobufSwerveDriveKinematics>(arena);
+}
+
+template <size_t NumModules>
+frc::SwerveDriveKinematics<NumModules>
+wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufSwerveDriveKinematics*>(&msg);
+  if (m->modules_size() != NumModules) {
+    throw std::invalid_argument(
+        fmt::format("Tried to unpack message with {} elements in modules into "
+                    "SwerveDriveKinematics with {} modules",
+                    m->modules_size(), NumModules));
+  }
+  return frc::SwerveDriveKinematics<NumModules>{
+      wpi::UnpackProtobufArray<wpi::proto::ProtobufTranslation2d,
+                               frc::Translation2d, NumModules>(m->modules())};
+}
+
+template <size_t NumModules>
+void wpi::Protobuf<frc::SwerveDriveKinematics<NumModules>>::Pack(
+    google::protobuf::Message* msg,
+    const frc::SwerveDriveKinematics<NumModules>& value) {
+  auto m = static_cast<wpi::proto::ProtobufSwerveDriveKinematics*>(msg);
+  wpi::PackProtobufArray(m->mutable_modules(), value.GetModules());
+}

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

@@ -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.
+
+#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 kTypeName = wpi::Concat(
+      "SwerveDriveKinematics__"_ct_string, wpi::NumToCtString<NumModules>());
+  static constexpr std::string_view GetTypeName() { return kTypeName; }
+  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 void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::SwerveDriveKinematics<4>>);
+static_assert(wpi::HasNestedStruct<frc::SwerveDriveKinematics<4>>);
+static_assert(wpi::StructSerializable<frc::SwerveDriveKinematics<3>>);
+static_assert(wpi::HasNestedStruct<frc::SwerveDriveKinematics<3>>);
+
+#include "frc/kinematics/struct/SwerveDriveKinematicsStruct.inc"

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

@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/kinematics/struct/SwerveDriveKinematicsStruct.h"
+
+template <size_t NumModules>
+frc::SwerveDriveKinematics<NumModules>
+wpi::Struct<frc::SwerveDriveKinematics<NumModules>>::Unpack(
+    std::span<const uint8_t> data) {
+  constexpr size_t kModulesOff = 0;
+  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) {
+  constexpr size_t kModulesOff = 0;
+  wpi::PackStructArray<kModulesOff, NumModules>(data, value.GetModules());
+}