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