[wpiutil, ntcore] Add structured data support (#5391)

This adds support for two serialization formats for complex data types:

- Protobuf for complex objects with variable length internals that need forward and backward wire compatibility (lower speed, more flexible)
- Raw struct (ByteBuffer-style) for fixed-length objects (higher speed, less flexible)

Deserialization can be done either by creating a new object (for immutable objects) or overwriting the contents of an existing object (for mutable objects).

Implementing classes should provide inner classes that implement the Protobuf or Struct interface (in Java) or specialize the wpi::Protobuf or wpi::Struct struct (in C++). It is possible for classes to implement both. If the class itself does not implement serialization, it's possible for third parties/users to provide an implementation instead.

Uses the Google protobuf implementation for C++ and the QuickBuffers alternative protobuf implementation for Java.

wpimath/src/main/native/cpp/geometry/Pose2d.cpp

@@ -9,6 +9,7 @@
 #include <wpi/json.h>
 
 #include "frc/MathUtil.h"
+#include "geometry2d.pb.h"
 
 using namespace frc;
 
@@ -108,3 +109,23 @@ void frc::from_json(const wpi::json& json, Pose2d& pose) {
   pose = Pose2d{json.at("translation").get<Translation2d>(),
                 json.at("rotation").get<Rotation2d>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
+      arena);
+}
+
+frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
+  return Pose2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+                wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
+                                      const frc::Pose2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

wpimath/src/main/native/cpp/geometry/Pose3d.cpp

@@ -9,6 +9,8 @@
 #include <Eigen/Core>
 #include <wpi/json.h>
 
+#include "geometry3d.pb.h"
+
 using namespace frc;
 
 namespace {
@@ -187,3 +189,23 @@ void frc::from_json(const wpi::json& json, Pose3d& pose) {
   pose = Pose3d{json.at("translation").get<Translation3d>(),
                 json.at("rotation").get<Rotation3d>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
+      arena);
+}
+
+frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
+  return Pose3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+                wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
+                                      const frc::Pose3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

wpimath/src/main/native/cpp/geometry/Quaternion.cpp

@@ -8,6 +8,8 @@
 
 #include <wpi/json.h>
 
+#include "geometry3d.pb.h"
+
 using namespace frc;
 
 Quaternion::Quaternion(double w, double x, double y, double z)
@@ -230,3 +232,24 @@ void frc::from_json(const wpi::json& json, Quaternion& quaternion) {
       Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
                  json.at("Y").get<double>(), json.at("Z").get<double>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Quaternion>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
+      arena);
+}
+
+frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
+  return frc::Quaternion{m->w(), m->x(), m->y(), m->z()};
+}
+
+void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
+                                          const frc::Quaternion& value) {
+  auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
+  m->set_w(value.W());
+  m->set_x(value.X());
+  m->set_y(value.Y());
+  m->set_z(value.Z());
+}

wpimath/src/main/native/cpp/geometry/Rotation2d.cpp

@@ -8,6 +8,7 @@
 
 #include <wpi/json.h>
 
+#include "geometry2d.pb.h"
 #include "units/math.h"
 
 using namespace frc;
@@ -19,3 +20,21 @@ void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
 void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
   rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
+      arena);
+}
+
+frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
+  return frc::Rotation2d{units::radian_t{m->value()}};
+}
+
+void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
+                                          const frc::Rotation2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
+  m->set_value(value.Radians().value());
+}

wpimath/src/main/native/cpp/geometry/Rotation3d.cpp

@@ -13,6 +13,7 @@
 #include <wpi/json.h>
 
 #include "frc/fmt/Eigen.h"
+#include "geometry3d.pb.h"
 #include "units/math.h"
 #include "wpimath/MathShared.h"
 
@@ -261,3 +262,21 @@ void frc::to_json(wpi::json& json, const Rotation3d& rotation) {
 void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
   rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
+      arena);
+}
+
+frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
+  return Rotation3d{wpi::UnpackProtobuf<frc::Quaternion>(m->q())};
+}
+
+void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
+                                          const frc::Rotation3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
+  wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
+}

wpimath/src/main/native/cpp/geometry/Transform2d.cpp

@@ -5,6 +5,7 @@
 #include "frc/geometry/Transform2d.h"
 
 #include "frc/geometry/Pose2d.h"
+#include "geometry2d.pb.h"
 
 using namespace frc;
 
@@ -21,3 +22,23 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) {
 Transform2d Transform2d::operator+(const Transform2d& other) const {
   return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTransform2d>(arena);
+}
+
+frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
+  return Transform2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+                     wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
+                                           const frc::Transform2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

wpimath/src/main/native/cpp/geometry/Transform3d.cpp

@@ -5,6 +5,7 @@
 #include "frc/geometry/Transform3d.h"
 
 #include "frc/geometry/Pose3d.h"
+#include "geometry3d.pb.h"
 
 using namespace frc;
 
@@ -35,3 +36,23 @@ Transform3d Transform3d::Inverse() const {
 Transform3d Transform3d::operator+(const Transform3d& other) const {
   return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTransform3d>(arena);
+}
+
+frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
+  return Transform3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+                     wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
+}
+
+void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
+                                           const frc::Transform3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

wpimath/src/main/native/cpp/geometry/Translation2d.cpp

@@ -6,6 +6,7 @@
 
 #include <wpi/json.h>
 
+#include "geometry2d.pb.h"
 #include "units/math.h"
 
 using namespace frc;
@@ -48,3 +49,22 @@ void frc::from_json(const wpi::json& json, Translation2d& translation) {
   translation = Translation2d{units::meter_t{json.at("x").get<double>()},
                               units::meter_t{json.at("y").get<double>()}};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTranslation2d>(arena);
+}
+
+frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
+  return frc::Translation2d{units::meter_t{m->x()}, units::meter_t{m->y()}};
+}
+
+void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
+                                             const frc::Translation2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
+  m->set_x(value.X().value());
+  m->set_y(value.Y().value());
+}

wpimath/src/main/native/cpp/geometry/Translation3d.cpp

@@ -6,6 +6,7 @@
 
 #include <wpi/json.h>
 
+#include "geometry3d.pb.h"
 #include "units/length.h"
 #include "units/math.h"
 
@@ -52,3 +53,24 @@ void frc::from_json(const wpi::json& json, Translation3d& translation) {
                               units::meter_t{json.at("y").get<double>()},
                               units::meter_t{json.at("z").get<double>()}};
 }
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTranslation3d>(arena);
+}
+
+frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
+  return frc::Translation3d{units::meter_t{m->x()}, units::meter_t{m->y()},
+                            units::meter_t{m->z()}};
+}
+
+void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
+                                             const frc::Translation3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
+  m->set_x(value.X().value());
+  m->set_y(value.Y().value());
+  m->set_z(value.Z().value());
+}

wpimath/src/main/native/cpp/geometry/Twist2d.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 "frc/geometry/Twist2d.h"
+
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+google::protobuf::Message* wpi::Protobuf<frc::Twist2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist2d>(
+      arena);
+}
+
+frc::Twist2d wpi::Protobuf<frc::Twist2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTwist2d*>(&msg);
+  return frc::Twist2d{units::meter_t{m->dx()}, units::meter_t{m->dy()},
+                      units::radian_t{m->dtheta()}};
+}
+
+void wpi::Protobuf<frc::Twist2d>::Pack(google::protobuf::Message* msg,
+                                       const frc::Twist2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTwist2d*>(msg);
+  m->set_dx(value.dx.value());
+  m->set_dy(value.dy.value());
+  m->set_dtheta(value.dtheta.value());
+}

wpimath/src/main/native/cpp/geometry/Twist3d.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 "frc/geometry/Twist3d.h"
+
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+google::protobuf::Message* wpi::Protobuf<frc::Twist3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist3d>(
+      arena);
+}
+
+frc::Twist3d wpi::Protobuf<frc::Twist3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTwist3d*>(&msg);
+  return frc::Twist3d{units::meter_t{m->dx()},  units::meter_t{m->dy()},
+                      units::meter_t{m->dz()},  units::radian_t{m->rx()},
+                      units::radian_t{m->ry()}, units::radian_t{m->rz()}};
+}
+
+void wpi::Protobuf<frc::Twist3d>::Pack(google::protobuf::Message* msg,
+                                       const frc::Twist3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTwist3d*>(msg);
+  m->set_dx(value.dx.value());
+  m->set_dy(value.dy.value());
+  m->set_dz(value.dz.value());
+  m->set_rx(value.rx.value());
+  m->set_ry(value.ry.value());
+  m->set_rz(value.rz.value());
+}

wpimath/src/main/native/include/frc/geometry/Pose2d.h

@@ -9,7 +9,10 @@
 
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
+#include "frc/geometry/Rotation2d.h"
 #include "frc/geometry/Transform2d.h"
 #include "frc/geometry/Translation2d.h"
 #include "frc/geometry/Twist2d.h"
@@ -212,4 +215,38 @@ void from_json(const wpi::json& json, Pose2d& pose);
 
 }  // namespace frc
 
+template <>
+struct wpi::Struct<frc::Pose2d> {
+  static constexpr std::string_view kTypeString = "struct:Pose2d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
+                                  wpi::Struct<frc::Rotation2d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation2d translation;Rotation2d rotation";
+  static frc::Pose2d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data, const frc::Pose2d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Pose2d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Pose2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Pose2d& value);
+};
+
 #include "frc/geometry/Pose2d.inc"

wpimath/src/main/native/include/frc/geometry/Pose3d.h

@@ -6,8 +6,11 @@
 
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation3d.h"
 #include "frc/geometry/Transform3d.h"
 #include "frc/geometry/Translation3d.h"
 #include "frc/geometry/Twist3d.h"
@@ -213,3 +216,37 @@ WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, Pose3d& pose);
 
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Pose3d> {
+  static constexpr std::string_view kTypeString = "struct:Pose3d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
+                                  wpi::Struct<frc::Rotation3d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation3d translation;Rotation3d rotation";
+  static frc::Pose3d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data, const frc::Pose3d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation3d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Pose3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Pose3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Pose3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Pose3d& value);
+};

wpimath/src/main/native/include/frc/geometry/Quaternion.h

@@ -7,6 +7,8 @@
 #include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 namespace frc {
 
@@ -187,3 +189,32 @@ WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, Quaternion& quaternion);
 
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Quaternion> {
+  static constexpr std::string_view kTypeString = "struct:Quaternion";
+  static constexpr size_t kSize = 32;
+  static constexpr std::string_view kSchema =
+      "double w;double x;double y;double z";
+  static frc::Quaternion Unpack(std::span<const uint8_t, 32> data) {
+    return {wpi::UnpackStruct<double, 0>(data),
+            wpi::UnpackStruct<double, 8>(data),
+            wpi::UnpackStruct<double, 16>(data),
+            wpi::UnpackStruct<double, 24>(data)};
+  }
+  static void Pack(std::span<uint8_t, 32> data, const frc::Quaternion& value) {
+    wpi::PackStruct<0>(data, value.W());
+    wpi::PackStruct<8>(data, value.X());
+    wpi::PackStruct<16>(data, value.Y());
+    wpi::PackStruct<24>(data, value.Z());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Quaternion> {
+  static constexpr std::string_view kTypeString = "proto:Quaternion";
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Quaternion Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Quaternion& value);
+};

wpimath/src/main/native/include/frc/geometry/Rotation2d.h

@@ -6,6 +6,8 @@
 
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "units/angle.h"
 
@@ -176,4 +178,25 @@ void from_json(const wpi::json& json, Rotation2d& rotation);
 
 }  // namespace frc
 
+template <>
+struct wpi::Struct<frc::Rotation2d> {
+  static constexpr std::string_view kTypeString = "struct:Rotation2d";
+  static constexpr size_t kSize = 8;
+  static constexpr std::string_view kSchema = "double value";
+  static frc::Rotation2d Unpack(std::span<const uint8_t, 8> data) {
+    return units::radian_t{wpi::UnpackStruct<double>(data)};
+  }
+  static void Pack(std::span<uint8_t, 8> data, const frc::Rotation2d& value) {
+    wpi::PackStruct(data, value.Radians().value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Rotation2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Rotation2d& value);
+};
+
 #include "frc/geometry/Rotation2d.inc"

wpimath/src/main/native/include/frc/geometry/Rotation3d.h

@@ -7,6 +7,8 @@
 #include <Eigen/Core>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Quaternion.h"
 #include "frc/geometry/Rotation2d.h"
@@ -194,3 +196,31 @@ WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, Rotation3d& rotation);
 
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Rotation3d> {
+  static constexpr std::string_view kTypeString = "struct:Rotation3d";
+  static constexpr size_t kSize = wpi::Struct<frc::Quaternion>::kSize;
+  static constexpr std::string_view kSchema = "Quaternion q";
+  static frc::Rotation3d Unpack(std::span<const uint8_t, kSize> data) {
+    return frc::Rotation3d{wpi::UnpackStruct<frc::Quaternion, 0>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data,
+                   const frc::Rotation3d& value) {
+    wpi::PackStruct<0>(data, value.GetQuaternion());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Quaternion>(fn);
+  }
+};
+
+static_assert(wpi::HasNestedStruct<frc::Rotation3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Rotation3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Rotation3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Rotation3d& value);
+};

wpimath/src/main/native/include/frc/geometry/Transform2d.h

@@ -5,6 +5,8 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Translation2d.h"
 
@@ -124,4 +126,40 @@ class WPILIB_DLLEXPORT Transform2d {
 };
 }  // namespace frc
 
+template <>
+struct wpi::Struct<frc::Transform2d> {
+  static constexpr std::string_view kTypeString = "struct:Transform2d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
+                                  wpi::Struct<frc::Rotation2d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation2d translation;Rotation2d rotation";
+  static frc::Transform2d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation2d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data,
+                   const frc::Transform2d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation2d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation2d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Transform2d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Transform2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Transform2d& value);
+};
+
 #include "frc/geometry/Transform2d.inc"

wpimath/src/main/native/include/frc/geometry/Transform3d.h

@@ -5,6 +5,8 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Translation3d.h"
 
@@ -129,3 +131,39 @@ class WPILIB_DLLEXPORT Transform3d {
   Rotation3d m_rotation;
 };
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Transform3d> {
+  static constexpr std::string_view kTypeString = "struct:Transform3d";
+  static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
+                                  wpi::Struct<frc::Rotation3d>::kSize;
+  static constexpr std::string_view kSchema =
+      "Translation3d translation;Rotation3d rotation";
+  static frc::Transform3d Unpack(std::span<const uint8_t, kSize> data) {
+    return {wpi::UnpackStruct<frc::Translation3d, 0>(data),
+            wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data)};
+  }
+  static void Pack(std::span<uint8_t, kSize> data,
+                   const frc::Transform3d& value) {
+    wpi::PackStruct<0>(data, value.Translation());
+    wpi::PackStruct<kRotationOff>(data, value.Rotation());
+  }
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::ForEachStructSchema<frc::Translation3d>(fn);
+    wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+  }
+
+ private:
+  static constexpr size_t kRotationOff = wpi::Struct<frc::Translation3d>::kSize;
+};
+
+static_assert(wpi::HasNestedStruct<frc::Transform3d>);
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Transform3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Transform3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Transform3d& value);
+};

wpimath/src/main/native/include/frc/geometry/Translation2d.h

@@ -9,6 +9,8 @@
 
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Rotation2d.h"
 #include "units/length.h"
@@ -198,4 +200,28 @@ void from_json(const wpi::json& json, Translation2d& state);
 
 }  // namespace frc
 
+template <>
+struct wpi::Struct<frc::Translation2d> {
+  static constexpr std::string_view kTypeString = "struct:Translation2d";
+  static constexpr size_t kSize = 16;
+  static constexpr std::string_view kSchema = "double x;double y";
+  static frc::Translation2d Unpack(std::span<const uint8_t, 16> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 16> data,
+                   const frc::Translation2d& value) {
+    wpi::PackStruct<0>(data, value.X().value());
+    wpi::PackStruct<8>(data, value.Y().value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Translation2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Translation2d& value);
+};
+
 #include "frc/geometry/Translation2d.inc"

wpimath/src/main/native/include/frc/geometry/Translation3d.h

@@ -6,6 +6,8 @@
 
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Rotation3d.h"
 #include "frc/geometry/Translation2d.h"
@@ -183,4 +185,30 @@ void from_json(const wpi::json& json, Translation3d& state);
 
 }  // namespace frc
 
+template <>
+struct wpi::Struct<frc::Translation3d> {
+  static constexpr std::string_view kTypeString = "struct:Translation3d";
+  static constexpr size_t kSize = 24;
+  static constexpr std::string_view kSchema = "double x;double y;double z";
+  static frc::Translation3d Unpack(std::span<const uint8_t, 24> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 16>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 24> data,
+                   const frc::Translation3d& value) {
+    wpi::PackStruct<0>(data, value.X().value());
+    wpi::PackStruct<8>(data, value.Y().value());
+    wpi::PackStruct<16>(data, value.Z().value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Translation3d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Translation3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg,
+                   const frc::Translation3d& value);
+};
+
 #include "frc/geometry/Translation3d.inc"

wpimath/src/main/native/include/frc/geometry/Twist2d.h

@@ -5,6 +5,8 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "units/angle.h"
 #include "units/length.h"
@@ -57,3 +59,28 @@ struct WPILIB_DLLEXPORT Twist2d {
   }
 };
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Twist2d> {
+  static constexpr std::string_view kTypeString = "struct:Twist2d";
+  static constexpr size_t kSize = 24;
+  static constexpr std::string_view kSchema =
+      "double dx;double dy;double dtheta";
+  static frc::Twist2d Unpack(std::span<const uint8_t, 24> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 16>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 24> data, const frc::Twist2d& value) {
+    wpi::PackStruct<0>(data, value.dx.value());
+    wpi::PackStruct<8>(data, value.dy.value());
+    wpi::PackStruct<16>(data, value.dtheta.value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist2d> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Twist2d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Twist2d& value);
+};

wpimath/src/main/native/include/frc/geometry/Twist3d.h

@@ -5,6 +5,8 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/protobuf/Protobuf.h>
+#include <wpi/struct/Struct.h>
 
 #include "frc/geometry/Rotation3d.h"
 #include "units/angle.h"
@@ -77,3 +79,35 @@ struct WPILIB_DLLEXPORT Twist3d {
   }
 };
 }  // namespace frc
+
+template <>
+struct wpi::Struct<frc::Twist3d> {
+  static constexpr std::string_view kTypeString = "struct:Twist3d";
+  static constexpr size_t kSize = 48;
+  static constexpr std::string_view kSchema =
+      "double dx;double dy;double dz;double rx;double ry;double rz";
+  static frc::Twist3d Unpack(std::span<const uint8_t, 48> data) {
+    return {units::meter_t{wpi::UnpackStruct<double, 0>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 8>(data)},
+            units::meter_t{wpi::UnpackStruct<double, 16>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 24>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 32>(data)},
+            units::radian_t{wpi::UnpackStruct<double, 40>(data)}};
+  }
+  static void Pack(std::span<uint8_t, 48> data, const frc::Twist3d& value) {
+    wpi::PackStruct<0>(data, value.dx.value());
+    wpi::PackStruct<8>(data, value.dy.value());
+    wpi::PackStruct<16>(data, value.dz.value());
+    wpi::PackStruct<24>(data, value.rx.value());
+    wpi::PackStruct<32>(data, value.ry.value());
+    wpi::PackStruct<40>(data, value.rz.value());
+  }
+};
+
+template <>
+struct WPILIB_DLLEXPORT wpi::Protobuf<frc::Twist3d> {
+  static constexpr std::string_view kTypeString = "proto:Twist3d";
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Twist3d Unpack(const google::protobuf::Message& msg);
+  static void Pack(google::protobuf::Message* msg, const frc::Twist3d& value);
+};