[wpimath] Move struct/proto classes to separate files (#5918)

Also add unit tests.

wpimath/src/test/native/cpp/geometry/proto/Pose2dProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Pose2d>;
+
+const Pose2d kExpectedData =
+    Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}};
+}  // namespace
+
+TEST(Pose2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Pose2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/proto/Pose3dProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Pose3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Pose3d>;
+
+const Pose3d kExpectedData =
+    Pose3d{Translation3d{1.1_m, 2.2_m, 1.1_m},
+           Rotation3d{Quaternion{1.91, 0.3504, 3.3, 1.74}}};
+}  // namespace
+
+TEST(Pose3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Pose3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/proto/QuaternionProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Quaternion.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Quaternion>;
+
+const Quaternion kExpectedData = Quaternion{1.1, 0.191, 35.04, 19.1};
+}  // namespace
+
+TEST(QuaternionProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Quaternion unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.W(), unpacked_data.W());
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}

wpimath/src/test/native/cpp/geometry/proto/Rotation2dProtoTest.cpp

@@ -0,0 +1,26 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Rotation2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Rotation2d>;
+
+const Rotation2d kExpectedData = Rotation2d{1.91_rad};
+}  // namespace
+
+TEST(Rotation2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Rotation2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Radians().value(), unpacked_data.Radians().value());
+}

wpimath/src/test/native/cpp/geometry/proto/Rotation3dProtoTest.cpp

@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Rotation3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Rotation3d>;
+
+const Rotation3d kExpectedData =
+    Rotation3d{Quaternion{2.29, 0.191, 0.191, 17.4}};
+}  // namespace
+
+TEST(Rotation3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Rotation3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.GetQuaternion(), unpacked_data.GetQuaternion());
+}

wpimath/src/test/native/cpp/geometry/proto/Transform2dProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Transform2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Transform2d>;
+
+const Transform2d kExpectedData =
+    Transform2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{4.4_rad}};
+}  // namespace
+
+TEST(Transform2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Transform2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/proto/Transform3dProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Transform3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Transform3d>;
+
+const Transform3d kExpectedData =
+    Transform3d{Translation3d{0.3504_m, 22.9_m, 3.504_m},
+                Rotation3d{Quaternion{0.3504, 35.04, 2.29, 0.3504}}};
+}  // namespace
+
+TEST(Transform3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Transform3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/proto/Translation2dProtoTest.cpp

@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Translation2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Translation2d>;
+
+const Translation2d kExpectedData = Translation2d{3.504_m, 22.9_m};
+}  // namespace
+
+TEST(Translation2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Translation2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.X().value(), unpacked_data.X().value());
+  EXPECT_EQ(kExpectedData.Y().value(), unpacked_data.Y().value());
+}

wpimath/src/test/native/cpp/geometry/proto/Translation3dProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Translation3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Translation3d>;
+
+const Translation3d kExpectedData = Translation3d{35.04_m, 22.9_m, 3.504_m};
+}  // namespace
+
+TEST(Translation3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Translation3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}

wpimath/src/test/native/cpp/geometry/proto/Twist2dProtoTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Twist2d.h"
+#include "geometry2d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Twist2d>;
+
+const Twist2d kExpectedData = Twist2d{2.29_m, 35.04_m, 35.04_rad};
+}  // namespace
+
+TEST(Twist2dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Twist2d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dtheta.value(), unpacked_data.dtheta.value());
+}

wpimath/src/test/native/cpp/geometry/proto/Twist3dProtoTest.cpp

@@ -0,0 +1,32 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Twist3d.h"
+#include "geometry3d.pb.h"
+
+using namespace frc;
+
+namespace {
+
+using ProtoType = wpi::Protobuf<frc::Twist3d>;
+
+const Twist3d kExpectedData =
+    Twist3d{1.1_m, 2.29_m, 35.04_m, 0.174_rad, 19.1_rad, 4.4_rad};
+}  // namespace
+
+TEST(Twist3dProtoTest, Roundtrip) {
+  google::protobuf::Arena arena;
+  google::protobuf::Message* proto = ProtoType::New(&arena);
+  ProtoType::Pack(proto, kExpectedData);
+
+  Twist3d unpacked_data = ProtoType::Unpack(*proto);
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dz.value(), unpacked_data.dz.value());
+  EXPECT_EQ(kExpectedData.rx.value(), unpacked_data.rx.value());
+  EXPECT_EQ(kExpectedData.ry.value(), unpacked_data.ry.value());
+  EXPECT_EQ(kExpectedData.rz.value(), unpacked_data.rz.value());
+}

wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp

@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Pose2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Pose2d>;
+const Pose2d kExpectedData{
+    Pose2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{22.9_rad}}};
+}  // namespace
+
+TEST(Pose2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Pose2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Pose3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Pose3d>;
+const Pose3d kExpectedData{
+    Pose3d{Translation3d{1.1_m, 2.2_m, 1.1_m},
+           Rotation3d{Quaternion{1.91, 0.3504, 3.3, 1.74}}}};
+}  // namespace
+
+TEST(Pose3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Pose3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Quaternion.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Quaternion>;
+const Quaternion kExpectedData{Quaternion{1.1, 0.191, 35.04, 19.1}};
+}  // namespace
+
+TEST(QuaternionStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Quaternion unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.W(), unpacked_data.W());
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}

wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Rotation2d>;
+const Rotation2d kExpectedData{Rotation2d{1.91_rad}};
+}  // namespace
+
+TEST(Rotation2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Rotation2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Radians(), unpacked_data.Radians());
+}

wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp

@@ -0,0 +1,26 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Rotation3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Rotation3d>;
+const Rotation3d kExpectedData{
+    Rotation3d{Quaternion{2.29, 0.191, 0.191, 17.4}}};
+}  // namespace
+
+TEST(Rotation3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Rotation3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.GetQuaternion(), unpacked_data.GetQuaternion());
+}

wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp

@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Transform2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Transform2d>;
+const Transform2d kExpectedData{
+    Transform2d{Translation2d{0.191_m, 2.2_m}, Rotation2d{4.4_rad}}};
+}  // namespace
+
+TEST(Transform2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Transform2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp

@@ -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.
+
+#include <gtest/gtest.h>
+
+#include "frc/geometry/Transform3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Transform3d>;
+const Transform3d kExpectedData{
+    Transform3d{Translation3d{0.3504_m, 22.9_m, 3.504_m},
+                Rotation3d{Quaternion{0.3504, 35.04, 2.29, 0.3504}}}};
+}  // namespace
+
+TEST(Transform3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Transform3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.Translation(), unpacked_data.Translation());
+  EXPECT_EQ(kExpectedData.Rotation(), unpacked_data.Rotation());
+}

wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp

@@ -0,0 +1,26 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Translation2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Translation2d>;
+const Translation2d kExpectedData{Translation2d{3.504_m, 22.9_m}};
+}  // namespace
+
+TEST(Translation2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Translation2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+}

wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp

@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Translation3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Translation3d>;
+const Translation3d kExpectedData{Translation3d{35.04_m, 22.9_m, 3.504_m}};
+}  // namespace
+
+TEST(Translation3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Translation3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.X(), unpacked_data.X());
+  EXPECT_EQ(kExpectedData.Y(), unpacked_data.Y());
+  EXPECT_EQ(kExpectedData.Z(), unpacked_data.Z());
+}

wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp

@@ -0,0 +1,27 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Twist2d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Twist2d>;
+const Twist2d kExpectedData{Twist2d{2.29_m, 35.04_m, 35.04_rad}};
+}  // namespace
+
+TEST(Twist2dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Twist2d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dtheta.value(), unpacked_data.dtheta.value());
+}

wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp

@@ -0,0 +1,31 @@
+// 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 <gtest/gtest.h>
+
+#include "frc/geometry/Twist3d.h"
+
+using namespace frc;
+
+namespace {
+
+using StructType = wpi::Struct<frc::Twist3d>;
+const Twist3d kExpectedData{
+    Twist3d{1.1_m, 2.29_m, 35.04_m, 0.174_rad, 19.1_rad, 4.4_rad}};
+}  // namespace
+
+TEST(Twist3dStructTest, Roundtrip) {
+  uint8_t buffer[StructType::kSize];
+  std::memset(buffer, 0, StructType::kSize);
+  StructType::Pack(buffer, kExpectedData);
+
+  Twist3d unpacked_data = StructType::Unpack(buffer);
+
+  EXPECT_EQ(kExpectedData.dx.value(), unpacked_data.dx.value());
+  EXPECT_EQ(kExpectedData.dy.value(), unpacked_data.dy.value());
+  EXPECT_EQ(kExpectedData.dz.value(), unpacked_data.dz.value());
+  EXPECT_EQ(kExpectedData.rx.value(), unpacked_data.rx.value());
+  EXPECT_EQ(kExpectedData.ry.value(), unpacked_data.ry.value());
+  EXPECT_EQ(kExpectedData.rz.value(), unpacked_data.rz.value());
+}