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

wpimath/src/main/native/include/frc/controller/DifferentialDriveFeedforward.h

@@ -79,5 +79,13 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
       units::meters_per_second_t nextLeftVelocity,
       units::meters_per_second_t currentRightVelocity,
       units::meters_per_second_t nextRightVelocity, units::second_t dt);
+
+  decltype(1_V / 1_mps) m_kVLinear;
+  decltype(1_V / 1_mps_sq) m_kALinear;
+  decltype(1_V / 1_mps) m_kVAngular;
+  decltype(1_V / 1_mps_sq) m_kAAngular;
 };
 }  // namespace frc
+
+#include "frc/controller/proto/DifferentialDriveFeedforwardProto.h"
+#include "frc/controller/struct/DifferentialDriveFeedforwardStruct.h"

wpimath/src/main/native/include/frc/controller/proto/DifferentialDriveFeedforwardProto.h

@@ -0,0 +1,19 @@
+// 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/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/controller/DifferentialDriveFeedforward.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::DifferentialDriveFeedforward> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::DifferentialDriveFeedforward Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::DifferentialDriveFeedforward& value);
+};

wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.h

@@ -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.
+
+#pragma once
+
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "units/length.h"
+
+// Everything is converted into units for
+// frc::SimpleMotorFeedforward<units::meters>
+
+template <class Distance>
+struct wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::SimpleMotorFeedforward<Distance> Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::SimpleMotorFeedforward<Distance>& value);
+};
+
+#include "frc/controller/proto/SimpleMotorFeedforwardProto.inc"

wpimath/src/main/native/include/frc/controller/proto/SimpleMotorFeedforwardProto.inc

@@ -0,0 +1,45 @@
+// 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/ProtoHelper.h>
+
+#include "controller.pb.h"
+#include "frc/controller/proto/SimpleMotorFeedforwardProto.h"
+
+template <class Distance>
+google::protobuf::Message*
+wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>>::New(
+    google::protobuf::Arena* arena) {
+  return wpi::CreateMessage<wpi::proto::ProtobufSimpleMotorFeedforward>(arena);
+}
+
+template <class Distance>
+frc::SimpleMotorFeedforward<Distance>
+wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufSimpleMotorFeedforward*>(&msg);
+  return {units::volt_t{m->ks()},
+          units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
+              m->kv()},
+          units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
+              m->ka()},
+          units::second_t{m->dt()}};
+}
+
+template <class Distance>
+void wpi::Protobuf<frc::SimpleMotorFeedforward<Distance>>::Pack(
+    google::protobuf::Message* msg,
+    const frc::SimpleMotorFeedforward<Distance>& value) {
+  auto m = static_cast<wpi::proto::ProtobufSimpleMotorFeedforward*>(msg);
+  m->set_ks(value.GetKs().value());
+  m->set_kv(units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
+      value.GetKv()}
+                .value());
+  m->set_ka(units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
+      value.GetKa()}
+                .value());
+  m->set_dt(units::second_t{value.GetDt()}.value());
+}

wpimath/src/main/native/include/frc/controller/struct/DifferentialDriveFeedforwardStruct.h

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/controller/DifferentialDriveFeedforward.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveFeedforward> {
+  static constexpr std::string_view GetTypeName() {
+    return "DifferentialDriveFeedforward";
+  }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double kv_linear;double ka_linear;double kv_angular;double "
+           "ka_angular";
+  }
+
+  static frc::DifferentialDriveFeedforward Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::DifferentialDriveFeedforward& value);
+};
+
+static_assert(wpi::StructSerializable<frc::DifferentialDriveFeedforward>);

wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.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 <wpi/struct/Struct.h>
+
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "units/length.h"
+
+// Everything is converted into units for
+// frc::SimpleMotorFeedforward<units::meters>
+
+template <class Distance>
+struct wpi::Struct<frc::SimpleMotorFeedforward<Distance>> {
+  static constexpr std::string_view GetTypeName() {
+    return "SimpleMotorFeedforward";
+  }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double ks;double kv;double ka;double dt";
+  }
+
+  static frc::SimpleMotorFeedforward<Distance> Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::SimpleMotorFeedforward<Distance>& value);
+};
+
+static_assert(
+    wpi::StructSerializable<frc::SimpleMotorFeedforward<units::meters>>);
+static_assert(
+    wpi::StructSerializable<frc::SimpleMotorFeedforward<units::feet>>);
+
+#include "frc/controller/struct/SimpleMotorFeedforwardStruct.inc"

wpimath/src/main/native/include/frc/controller/struct/SimpleMotorFeedforwardStruct.inc

@@ -0,0 +1,45 @@
+// 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/controller/struct/SimpleMotorFeedforwardStruct.h"
+
+template <class Distance>
+frc::SimpleMotorFeedforward<Distance>
+wpi::Struct<frc::SimpleMotorFeedforward<Distance>>::Unpack(
+    std::span<const uint8_t> data) {
+  constexpr size_t kKsOff = 0;
+  constexpr size_t kKvOff = kKsOff + 8;
+  constexpr size_t kKaOff = kKvOff + 8;
+  constexpr size_t kDtOff = kKaOff + 8;
+  return {units::volt_t{wpi::UnpackStruct<double, kKsOff>(data)},
+          units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
+              wpi::UnpackStruct<double, kKvOff>(data)},
+          units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
+              wpi::UnpackStruct<double, kKaOff>(data)},
+          units::second_t{wpi::UnpackStruct<double, kDtOff>(data)}};
+}
+
+template <class Distance>
+void wpi::Struct<frc::SimpleMotorFeedforward<Distance>>::Pack(
+    std::span<uint8_t> data,
+    const frc::SimpleMotorFeedforward<Distance>& value) {
+  constexpr size_t kKsOff = 0;
+  constexpr size_t kKvOff = kKsOff + 8;
+  constexpr size_t kKaOff = kKvOff + 8;
+  constexpr size_t kDtOff = kKaOff + 8;
+  wpi::PackStruct<kKsOff>(data, value.GetKs().value());
+  wpi::PackStruct<kKvOff>(
+      data,
+      units::unit_t<frc::SimpleMotorFeedforward<units::meters>::kv_unit>{
+          value.GetKv()}
+          .value());
+  wpi::PackStruct<kKaOff>(
+      data,
+      units::unit_t<frc::SimpleMotorFeedforward<units::meters>::ka_unit>{
+          value.GetKa()}
+          .value());
+  wpi::PackStruct<kDtOff>(data, units::second_t{value.GetDt()}.value());
+}

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

wpimath/src/main/native/include/frc/proto/MatrixProto.h

@@ -0,0 +1,22 @@
+// 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/EigenCore.h"
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+struct wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(
+      google::protobuf::Message* msg,
+      const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value);
+};
+
+#include "frc/proto/MatrixProto.inc"

wpimath/src/main/native/include/frc/proto/MatrixProto.inc

@@ -0,0 +1,60 @@
+// 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/proto/MatrixProto.h"
+#include "wpimath.pb.h"
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+google::protobuf::Message*
+wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::New(
+    google::protobuf::Arena* arena) {
+  return wpi::CreateMessage<wpi::proto::ProtobufMatrix>(arena);
+}
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>
+wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufMatrix*>(&msg);
+  if (m->num_rows() != Rows || m->num_cols() != Cols) {
+    throw std::invalid_argument(
+        fmt::format("Tried to unpack message with {} rows and {} columns into "
+                    "Matrix with {} rows and {} columns",
+                    m->num_rows(), m->num_cols(), Rows, Cols));
+  }
+  if (m->data_size() != Rows * Cols) {
+    throw std::invalid_argument(
+        fmt::format("Tried to unpack message with {} elements in data into "
+                    "Matrix with {} elements",
+                    m->data_size(), Rows * Cols));
+  }
+  frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> mat;
+  for (int i = 0; i < Rows * Cols; i++) {
+    mat(i) = m->data(i);
+  }
+  return mat;
+}
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+void wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Pack(
+    google::protobuf::Message* msg,
+    const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value) {
+  auto m = static_cast<wpi::proto::ProtobufMatrix*>(msg);
+  m->set_num_rows(Rows);
+  m->set_num_cols(Cols);
+  m->clear_data();
+  for (int i = 0; i < Rows * Cols; i++) {
+    m->add_data(value(i));
+  }
+}

wpimath/src/main/native/include/frc/proto/VectorProto.h

@@ -0,0 +1,21 @@
+// 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/EigenCore.h"
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+struct wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(
+      google::protobuf::Message* msg,
+      const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value);
+};
+
+#include "frc/proto/VectorProto.inc"

wpimath/src/main/native/include/frc/proto/VectorProto.inc

@@ -0,0 +1,49 @@
+// 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/proto/VectorProto.h"
+#include "wpimath.pb.h"
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+google::protobuf::Message*
+wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::New(
+    google::protobuf::Arena* arena) {
+  return wpi::CreateMessage<wpi::proto::ProtobufVector>(arena);
+}
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>
+wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufVector*>(&msg);
+  if (m->rows_size() != Size) {
+    throw std::invalid_argument(
+        fmt::format("Tried to unpack message with {} elements in rows into "
+                    "Vector with {} rows",
+                    m->rows_size(), Size));
+  }
+  frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> vec;
+  for (int i = 0; i < Size; i++) {
+    vec(i) = m->rows(i);
+  }
+  return vec;
+}
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+void wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Pack(
+    google::protobuf::Message* msg,
+    const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value) {
+  auto m = static_cast<wpi::proto::ProtobufVector*>(msg);
+  m->clear_rows();
+  for (int i = 0; i < Size; i++) {
+    m->add_rows(value(i));
+  }
+}

wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h

@@ -114,3 +114,6 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
   }
 };
 }  // namespace frc
+
+#include "frc/spline/proto/CubicHermiteSplineProto.h"
+#include "frc/spline/struct/CubicHermiteSplineStruct.h"

wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h

@@ -124,3 +124,6 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
   }
 };
 }  // namespace frc
+
+#include "frc/spline/proto/QuinticHermiteSplineProto.h"
+#include "frc/spline/struct/QuinticHermiteSplineStruct.h"

wpimath/src/main/native/include/frc/spline/proto/CubicHermiteSplineProto.h

@@ -0,0 +1,18 @@
+// 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/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/spline/CubicHermiteSpline.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::CubicHermiteSpline> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::CubicHermiteSpline Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::CubicHermiteSpline& value);
+};

wpimath/src/main/native/include/frc/spline/proto/QuinticHermiteSplineProto.h

@@ -0,0 +1,18 @@
+// 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/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+
+#include "frc/spline/QuinticHermiteSpline.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::QuinticHermiteSpline> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::QuinticHermiteSpline Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::QuinticHermiteSpline& value);
+};

wpimath/src/main/native/include/frc/spline/struct/CubicHermiteSplineStruct.h

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/spline/CubicHermiteSpline.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::CubicHermiteSpline> {
+  static constexpr std::string_view GetTypeName() {
+    return "CubicHermiteSpline";
+  }
+  static constexpr size_t GetSize() { return 4 * 2 * 8; }
+  static constexpr std::string_view GetSchema() {
+    return "double xInitial[2];double xFinal[2];double yInitial[2];double "
+           "yFinal[2]";
+  }
+
+  static frc::CubicHermiteSpline Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::CubicHermiteSpline& value);
+};
+
+static_assert(wpi::StructSerializable<frc::CubicHermiteSpline>);

wpimath/src/main/native/include/frc/spline/struct/QuinticHermiteSplineStruct.h

@@ -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.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/spline/QuinticHermiteSpline.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Struct<frc::QuinticHermiteSpline> {
+  static constexpr std::string_view GetTypeName() {
+    return "QuinticHermiteSpline";
+  }
+  static constexpr size_t GetSize() { return 4 * 3 * 8; }
+  static constexpr std::string_view GetSchema() {
+    return "double xInitial[3];double xFinal[3];double yInitial[3];double "
+           "yFinal[3]";
+  }
+
+  static frc::QuinticHermiteSpline Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::QuinticHermiteSpline& value);
+};
+
+static_assert(wpi::StructSerializable<frc::QuinticHermiteSpline>);

wpimath/src/main/native/include/frc/struct/MatrixStruct.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/EigenCore.h"
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+struct wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
+  static constexpr ct_string kTypeName =
+      wpi::Concat("Matrix__"_ct_string, wpi::NumToCtString<Rows>(),
+                  "_"_ct_string, wpi::NumToCtString<Cols>());
+  static constexpr std::string_view GetTypeName() { return kTypeName; }
+  static constexpr size_t GetSize() { return Rows * Cols * 8; }
+  static constexpr ct_string kSchema =
+      wpi::Concat("double data["_ct_string, wpi::NumToCtString<Rows * Cols>(),
+                  "]"_ct_string);
+  static constexpr std::string_view GetSchema() { return kSchema; }
+
+  static frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(
+      std::span<uint8_t> data,
+      const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Matrixd<1, 2>>);
+static_assert(wpi::StructSerializable<frc::Matrixd<3, 3>>);
+
+#include "frc/struct/MatrixStruct.inc"

wpimath/src/main/native/include/frc/struct/MatrixStruct.inc

@@ -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.
+
+#pragma once
+
+#include "frc/struct/MatrixStruct.h"
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>
+wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Unpack(
+    std::span<const uint8_t> data) {
+  constexpr size_t kDataOff = 0;
+  wpi::array<double, Rows * Cols> mat_data =
+      wpi::UnpackStructArray<double, kDataOff, Rows * Cols>(data);
+  frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> mat;
+  for (int i = 0; i < Rows * Cols; i++) {
+    mat(i) = mat_data[i];
+  }
+  return mat;
+}
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+void wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Pack(
+    std::span<uint8_t> data,
+    const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value) {
+  constexpr size_t kDataOff = 0;
+  wpi::array<double, Rows * Cols> mat_data(wpi::empty_array);
+  for (int i = 0; i < Rows * Cols; i++) {
+    mat_data[i] = value(i);
+  }
+  wpi::PackStructArray<kDataOff, Rows * Cols>(data, mat_data);
+}

wpimath/src/main/native/include/frc/struct/VectorStruct.h

@@ -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.
+
+#pragma once
+
+#include <fmt/format.h>
+#include <wpi/ct_string.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/EigenCore.h"
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+struct wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
+  static constexpr ct_string kTypeName =
+      wpi::Concat("Vector__"_ct_string, wpi::NumToCtString<Size>());
+  static constexpr std::string_view GetTypeName() { return kTypeName; }
+  static constexpr size_t GetSize() { return Size * 8; }
+  static constexpr ct_string kSchema = wpi::Concat(
+      "double data["_ct_string, wpi::NumToCtString<Size>(), "]"_ct_string);
+  static constexpr std::string_view GetSchema() { return kSchema; }
+
+  static frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(
+      std::span<uint8_t> data,
+      const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Vectord<1>>);
+static_assert(wpi::StructSerializable<frc::Vectord<3>>);
+
+#include "frc/struct/VectorStruct.inc"

wpimath/src/main/native/include/frc/struct/VectorStruct.inc

@@ -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.
+
+#pragma once
+
+#include "frc/struct/VectorStruct.h"
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>
+wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Unpack(
+    std::span<const uint8_t> data) {
+  constexpr size_t kDataOff = 0;
+  wpi::array<double, Size> vec_data =
+      wpi::UnpackStructArray<double, kDataOff, Size>(data);
+  frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> vec;
+  for (int i = 0; i < Size; i++) {
+    vec(i) = vec_data[i];
+  }
+  return vec;
+}
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+void wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Pack(
+    std::span<uint8_t> data,
+    const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value) {
+  constexpr size_t kDataOff = 0;
+  wpi::array<double, Size> vec_data(wpi::empty_array);
+  for (int i = 0; i < Size; i++) {
+    vec_data[i] = value(i);
+  }
+  wpi::PackStructArray<kDataOff, Size>(data, vec_data);
+}

wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.h

@@ -0,0 +1,21 @@
+// 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/proto/MatrixProto.h"
+#include "frc/system/LinearSystem.h"
+
+template <int States, int Inputs, int Outputs>
+struct wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::LinearSystem<States, Inputs, Outputs> Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::LinearSystem<States, Inputs, Outputs>& value);
+};
+
+#include "frc/system/proto/LinearSystemProto.inc"

wpimath/src/main/native/include/frc/system/proto/LinearSystemProto.inc

@@ -0,0 +1,54 @@
+// 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/system/proto/LinearSystemProto.h"
+#include "system.pb.h"
+
+template <int States, int Inputs, int Outputs>
+google::protobuf::Message*
+wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::New(
+    google::protobuf::Arena* arena) {
+  return wpi::CreateMessage<wpi::proto::ProtobufLinearSystem>(arena);
+}
+
+template <int States, int Inputs, int Outputs>
+frc::LinearSystem<States, Inputs, Outputs>
+wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufLinearSystem*>(&msg);
+  if (m->num_states() != States || m->num_inputs() != Inputs ||
+      m->num_outputs() != Outputs) {
+    throw std::invalid_argument(fmt::format(
+        "Tried to unpack message with {} states and {} inputs and {} outputs "
+        "into LinearSystem with {} states and {} inputs and {} outputs",
+        m->num_states(), m->num_inputs(), m->num_outputs(), States, Inputs,
+        Outputs));
+  }
+  return frc::LinearSystem<States, Inputs, Outputs>{
+      wpi::UnpackProtobuf<frc::Matrixd<States, States>>(m->a()),
+      wpi::UnpackProtobuf<frc::Matrixd<States, Inputs>>(m->b()),
+      wpi::UnpackProtobuf<frc::Matrixd<Outputs, States>>(m->c()),
+      wpi::UnpackProtobuf<frc::Matrixd<Outputs, Inputs>>(m->d())};
+}
+
+template <int States, int Inputs, int Outputs>
+void wpi::Protobuf<frc::LinearSystem<States, Inputs, Outputs>>::Pack(
+    google::protobuf::Message* msg,
+    const frc::LinearSystem<States, Inputs, Outputs>& value) {
+  auto m = static_cast<wpi::proto::ProtobufLinearSystem*>(msg);
+  m->set_num_states(States);
+  m->set_num_inputs(Inputs);
+  m->set_num_outputs(Outputs);
+  wpi::PackProtobuf(m->mutable_a(), value.A());
+  wpi::PackProtobuf(m->mutable_b(), value.B());
+  wpi::PackProtobuf(m->mutable_c(), value.C());
+  wpi::PackProtobuf(m->mutable_d(), value.D());
+}

wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.h

@@ -0,0 +1,52 @@
+// 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/struct/MatrixStruct.h"
+#include "frc/system/LinearSystem.h"
+
+template <int States, int Inputs, int Outputs>
+struct wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>> {
+  static constexpr ct_string kTypeName =
+      wpi::Concat("LinearSystem__"_ct_string, wpi::NumToCtString<States>(),
+                  "_"_ct_string, wpi::NumToCtString<Inputs>(), "_"_ct_string,
+                  wpi::NumToCtString<Outputs>());
+  static constexpr std::string_view GetTypeName() { return kTypeName; }
+  static constexpr size_t GetSize() {
+    return wpi::Struct<frc::Matrixd<States, States>>::GetSize() +
+           wpi::Struct<frc::Matrixd<States, Inputs>>::GetSize() +
+           wpi::Struct<frc::Matrixd<Outputs, States>>::GetSize() +
+           wpi::Struct<frc::Matrixd<Outputs, Inputs>>::GetSize();
+  }
+  static constexpr ct_string kSchema = wpi::Concat(
+      wpi::Struct<frc::Matrixd<States, States>>::kTypeName, " a;"_ct_string,
+      wpi::Struct<frc::Matrixd<States, Inputs>>::kTypeName, " b;"_ct_string,
+      wpi::Struct<frc::Matrixd<Outputs, States>>::kTypeName, " c;"_ct_string,
+      wpi::Struct<frc::Matrixd<Outputs, Inputs>>::kTypeName, " d"_ct_string);
+  static constexpr std::string_view GetSchema() { return kSchema; }
+
+  static frc::LinearSystem<States, Inputs, Outputs> Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const frc::LinearSystem<States, Inputs, Outputs>& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Matrixd<States, States>>(fn);
+    wpi::ForEachStructSchema<frc::Matrixd<States, Inputs>>(fn);
+    wpi::ForEachStructSchema<frc::Matrixd<Outputs, States>>(fn);
+    wpi::ForEachStructSchema<frc::Matrixd<Outputs, Inputs>>(fn);
+  }
+};
+
+static_assert(wpi::StructSerializable<frc::LinearSystem<4, 3, 2>>);
+static_assert(wpi::HasNestedStruct<frc::LinearSystem<4, 3, 2>>);
+static_assert(wpi::StructSerializable<frc::LinearSystem<2, 3, 4>>);
+static_assert(wpi::HasNestedStruct<frc::LinearSystem<2, 3, 4>>);
+
+#include "frc/system/struct/LinearSystemStruct.inc"

wpimath/src/main/native/include/frc/system/struct/LinearSystemStruct.inc

@@ -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.
+
+#pragma once
+
+#include "frc/system/struct/LinearSystemStruct.h"
+
+template <int States, int Inputs, int Outputs>
+frc::LinearSystem<States, Inputs, Outputs>
+wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>>::Unpack(
+    std::span<const uint8_t> data) {
+  constexpr size_t kAOff = 0;
+  constexpr size_t kBOff =
+      kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
+  constexpr size_t kCOff =
+      kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
+  constexpr size_t kDOff =
+      kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
+  return frc::LinearSystem<States, Inputs, Outputs>{
+      wpi::UnpackStruct<frc::Matrixd<States, States>, kAOff>(data),
+      wpi::UnpackStruct<frc::Matrixd<States, Inputs>, kBOff>(data),
+      wpi::UnpackStruct<frc::Matrixd<Outputs, States>, kCOff>(data),
+      wpi::UnpackStruct<frc::Matrixd<Outputs, Inputs>, kDOff>(data)};
+}
+
+template <int States, int Inputs, int Outputs>
+void wpi::Struct<frc::LinearSystem<States, Inputs, Outputs>>::Pack(
+    std::span<uint8_t> data,
+    const frc::LinearSystem<States, Inputs, Outputs>& value) {
+  constexpr size_t kAOff = 0;
+  constexpr size_t kBOff =
+      kAOff + wpi::GetStructSize<frc::Matrixd<States, States>>();
+  constexpr size_t kCOff =
+      kBOff + wpi::GetStructSize<frc::Matrixd<States, Inputs>>();
+  constexpr size_t kDOff =
+      kCOff + wpi::GetStructSize<frc::Matrixd<Outputs, States>>();
+  wpi::PackStruct<kAOff>(data, value.A());
+  wpi::PackStruct<kBOff>(data, value.B());
+  wpi::PackStruct<kCOff>(data, value.C());
+  wpi::PackStruct<kDOff>(data, value.D());
+}