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