From a583ca01e1139ecd7b76560a4537dfeb2080b172 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sat, 2 Dec 2023 23:36:44 -0800 Subject: [PATCH] [wpiutil] Change Struct to allow non-constexpr implementation (#5992) This required changing the constant values (e.g. kSize) into functions (e.g. GetSize()). Fixed implementations of ForEachNested to be inline (as these are actually templates). Also added a ntcore Struct test. --- .../include/networktables/StructArrayTopic.h | 29 +- .../include/networktables/StructTopic.h | 54 ++-- ntcore/src/test/native/cpp/StructTest.cpp | 296 +++++++++++++++++ .../cpp/geometry/struct/Pose2dStruct.cpp | 13 +- .../cpp/geometry/struct/Pose3dStruct.cpp | 13 +- .../cpp/geometry/struct/QuaternionStruct.cpp | 5 +- .../cpp/geometry/struct/Rotation2dStruct.cpp | 5 +- .../cpp/geometry/struct/Rotation3dStruct.cpp | 10 +- .../cpp/geometry/struct/Transform2dStruct.cpp | 13 +- .../cpp/geometry/struct/Transform3dStruct.cpp | 13 +- .../geometry/struct/Translation2dStruct.cpp | 4 +- .../geometry/struct/Translation3dStruct.cpp | 4 +- .../cpp/geometry/struct/Twist2dStruct.cpp | 5 +- .../cpp/geometry/struct/Twist3dStruct.cpp | 5 +- .../kinematics/struct/ChassisSpeedsStruct.cpp | 4 +- .../DifferentialDriveKinematicsStruct.cpp | 4 +- .../DifferentialDriveWheelSpeedsStruct.cpp | 4 +- .../struct/MecanumDriveKinematicsStruct.cpp | 16 +- .../MecanumDriveWheelPositionsStruct.cpp | 4 +- .../struct/MecanumDriveWheelSpeedsStruct.cpp | 5 +- .../struct/SwerveModulePositionStruct.cpp | 10 +- .../struct/SwerveModuleStateStruct.cpp | 10 +- .../frc/geometry/struct/Pose2dStruct.h | 22 +- .../frc/geometry/struct/Pose3dStruct.h | 22 +- .../frc/geometry/struct/QuaternionStruct.h | 16 +- .../frc/geometry/struct/Rotation2dStruct.h | 13 +- .../frc/geometry/struct/Rotation3dStruct.h | 19 +- .../frc/geometry/struct/Transform2dStruct.h | 25 +- .../frc/geometry/struct/Transform3dStruct.h | 25 +- .../frc/geometry/struct/Translation2dStruct.h | 13 +- .../frc/geometry/struct/Translation3dStruct.h | 15 +- .../frc/geometry/struct/Twist2dStruct.h | 13 +- .../frc/geometry/struct/Twist3dStruct.h | 13 +- .../kinematics/struct/ChassisSpeedsStruct.h | 16 +- .../DifferentialDriveKinematicsStruct.h | 14 +- .../DifferentialDriveWheelSpeedsStruct.h | 15 +- .../struct/MecanumDriveKinematicsStruct.h | 25 +- .../struct/MecanumDriveWheelPositionsStruct.h | 18 +- .../struct/MecanumDriveWheelSpeedsStruct.h | 18 +- .../struct/SwerveModulePositionStruct.h | 21 +- .../struct/SwerveModuleStateStruct.h | 20 +- .../cpp/geometry/struct/Pose2dStructTest.cpp | 4 +- .../cpp/geometry/struct/Pose3dStructTest.cpp | 4 +- .../geometry/struct/QuaternionStructTest.cpp | 4 +- .../geometry/struct/Rotation2dStructTest.cpp | 4 +- .../geometry/struct/Rotation3dStructTest.cpp | 4 +- .../geometry/struct/Transform2dStructTest.cpp | 4 +- .../geometry/struct/Transform3dStructTest.cpp | 4 +- .../struct/Translation2dStructTest.cpp | 4 +- .../struct/Translation3dStructTest.cpp | 4 +- .../cpp/geometry/struct/Twist2dStructTest.cpp | 4 +- .../cpp/geometry/struct/Twist3dStructTest.cpp | 4 +- .../struct/ChassisSpeedsStructTest.cpp | 4 +- .../DifferentialDriveKinematicsStructTest.cpp | 4 +- ...DifferentialDriveWheelSpeedsStructTest.cpp | 4 +- .../MecanumDriveKinematicsStructTest.cpp | 4 +- .../MecanumDriveWheelPositionsStructTest.cpp | 4 +- .../MecanumDriveWheelSpeedsStructTest.cpp | 4 +- .../struct/SwerveModulePositionStructTest.cpp | 4 +- .../struct/SwerveModuleStateStructTest.cpp | 4 +- wpiutil/src/main/native/include/wpi/DataLog.h | 13 +- .../main/native/include/wpi/struct/Struct.h | 304 ++++++++++-------- 62 files changed, 812 insertions(+), 450 deletions(-) create mode 100644 ntcore/src/test/native/cpp/StructTest.cpp diff --git a/ntcore/src/main/native/include/networktables/StructArrayTopic.h b/ntcore/src/main/native/include/networktables/StructArrayTopic.h index bb5bed2ef3..f3529eafd2 100644 --- a/ntcore/src/main/native/include/networktables/StructArrayTopic.h +++ b/ntcore/src/main/native/include/networktables/StructArrayTopic.h @@ -7,9 +7,9 @@ #include #include +#include #include #include -#include #include #include @@ -121,16 +121,17 @@ class StructArraySubscriber : public Subscriber { #endif TimestampedValueType GetAtomic(U&& defaultValue) const { wpi::SmallVector buf; + size_t size = S::GetSize(); TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {}); - if (view.value.size() == 0 || (view.value.size() % S::kSize) != 0) { + if (view.value.size() == 0 || (view.value.size() % size) != 0) { return {0, 0, std::forward(defaultValue)}; } TimestampedValueType rv{view.time, view.serverTime, {}}; - rv.value.reserve(view.value.size() / S::kSize); + rv.value.reserve(view.value.size() / size); for (auto in = view.value.begin(), end = view.value.end(); in != end; - in += S::kSize) { + in += size) { rv.value.emplace_back( - S::Unpack(std::span{in, in + S::kSize})); + S::Unpack(std::span{std::to_address(in), size})); } return rv; } @@ -145,16 +146,17 @@ class StructArraySubscriber : public Subscriber { */ TimestampedValueType GetAtomic(std::span defaultValue) const { wpi::SmallVector buf; + size_t size = S::GetSize(); TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {}); - if (view.value.size() == 0 || (view.value.size() % S::kSize) != 0) { + if (view.value.size() == 0 || (view.value.size() % size) != 0) { return {0, 0, {defaultValue.begin(), defaultValue.end()}}; } TimestampedValueType rv{view.time, view.serverTime, {}}; - rv.value.reserve(view.value.size() / S::kSize); + rv.value.reserve(view.value.size() / size); for (auto in = view.value.begin(), end = view.value.end(); in != end; - in += S::kSize) { + in += size) { rv.value.emplace_back( - S::Unpack(std::span{in, in + S::kSize})); + S::Unpack(std::span{std::to_address(in), size})); } return rv; } @@ -174,16 +176,17 @@ class StructArraySubscriber : public Subscriber { auto raw = ::nt::ReadQueueRaw(m_subHandle); std::vector rv; rv.reserve(raw.size()); + size_t size = S::GetSize(); for (auto&& r : raw) { - if (r.value.size() == 0 || (r.value.size() % S::kSize) != 0) { + if (r.value.size() == 0 || (r.value.size() % size) != 0) { continue; } std::vector values; - values.reserve(r.value.size() / S::kSize); + values.reserve(r.value.size() / size); for (auto in = r.value.begin(), end = r.value.end(); in != end; - in += S::kSize) { + in += size) { values.emplace_back( - S::Unpack(std::span{in, in + S::kSize})); + S::Unpack(std::span{std::to_address(in), size})); } rv.emplace_back(r.time, r.serverTime, std::move(values)); } diff --git a/ntcore/src/main/native/include/networktables/StructTopic.h b/ntcore/src/main/native/include/networktables/StructTopic.h index d035d3b681..08d7a770b0 100644 --- a/ntcore/src/main/native/include/networktables/StructTopic.h +++ b/ntcore/src/main/native/include/networktables/StructTopic.h @@ -32,6 +32,13 @@ class StructTopic; template class StructSubscriber : public Subscriber { using S = wpi::Struct; + static constexpr size_t kBufSize = []() -> size_t { + if constexpr (wpi::is_constexpr([] { S::GetSize(); })) { + return S::GetSize(); + } else { + return 128; + } + }(); public: using TopicType = StructTopic; @@ -81,12 +88,12 @@ class StructSubscriber : public Subscriber { * @return true if successful */ bool GetInto(T* out) { - wpi::SmallVector buf; + wpi::SmallVector buf; TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {}); - if (view.value.size() < S::kSize) { + if (view.value.size() < S::GetSize()) { return false; } else { - wpi::UnpackStructInto(out, view.value.subspan<0, S::kSize>()); + wpi::UnpackStructInto(out, view.value); return true; } } @@ -109,13 +116,12 @@ class StructSubscriber : public Subscriber { * @return timestamped value */ TimestampedValueType GetAtomic(const T& defaultValue) const { - wpi::SmallVector buf; + wpi::SmallVector buf; TimestampedRawView view = ::nt::GetAtomicRaw(m_subHandle, buf, {}); - if (view.value.size() < S::kSize) { + if (view.value.size() < S::GetSize()) { return {0, 0, defaultValue}; } else { - return {view.time, view.serverTime, - S::Unpack(view.value.subspan<0, S::kSize>())}; + return {view.time, view.serverTime, S::Unpack(view.value)}; } } @@ -135,13 +141,11 @@ class StructSubscriber : public Subscriber { std::vector rv; rv.reserve(raw.size()); for (auto&& r : raw) { - if (r.value.size() < S::kSize) { + if (r.value.size() < S::GetSize()) { continue; } else { - rv.emplace_back( - r.time, r.serverTime, - S::Unpack( - std::span(r.value).subspan<0, S::kSize>())); + rv.emplace_back(r.time, r.serverTime, + S::Unpack(std::span(r.value))); } } return rv; @@ -208,9 +212,16 @@ class StructPublisher : public Publisher { if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) { GetTopic().GetInstance().template AddStructSchema(); } - uint8_t buf[S::kSize]; - S::Pack(buf, value); - ::nt::SetRaw(m_pubHandle, buf, time); + if constexpr (wpi::is_constexpr([] { S::GetSize(); })) { + uint8_t buf[S::GetSize()]; + S::Pack(buf, value); + ::nt::SetRaw(m_pubHandle, buf, time); + } else { + wpi::SmallVector buf; + buf.resize_for_overwrite(S::GetSize()); + S::Pack(buf, value); + ::nt::SetRaw(m_pubHandle, buf, time); + } } /** @@ -224,9 +235,16 @@ class StructPublisher : public Publisher { if (!m_schemaPublished.exchange(true, std::memory_order_relaxed)) { GetTopic().GetInstance().template AddStructSchema(); } - uint8_t buf[S::kSize]; - S::Pack(buf, value); - ::nt::SetDefaultRaw(m_pubHandle, buf); + if constexpr (wpi::is_constexpr([] { S::GetSize(); })) { + uint8_t buf[S::GetSize()]; + S::Pack(buf, value); + ::nt::SetDefaultRaw(m_pubHandle, buf); + } else { + wpi::SmallVector buf; + buf.resize_for_overwrite(S::GetSize()); + S::Pack(buf, value); + ::nt::SetDefaultRaw(m_pubHandle, buf); + } } /** diff --git a/ntcore/src/test/native/cpp/StructTest.cpp b/ntcore/src/test/native/cpp/StructTest.cpp new file mode 100644 index 0000000000..9cf0bc55f9 --- /dev/null +++ b/ntcore/src/test/native/cpp/StructTest.cpp @@ -0,0 +1,296 @@ +// 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 +#include +#include + +#include "networktables/NetworkTableInstance.h" +#include "networktables/StructArrayTopic.h" +#include "networktables/StructTopic.h" + +namespace { +struct Inner { + int a; + int b; +}; + +struct Outer { + Inner inner; + int c; +}; + +struct Inner2 { + int a; + int b; +}; + +struct Outer2 { + Inner2 inner; + int c; +}; +} // namespace + +template <> +struct wpi::Struct { + static constexpr std::string_view GetTypeString() { return "struct:Inner"; } + static constexpr size_t GetSize() { return 8; } + static constexpr std::string_view GetSchema() { return "int32 a; int32 b"; } + + static Inner Unpack(std::span data) { + return {wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}; + } + static void Pack(std::span data, const Inner& value) { + wpi::PackStruct<0>(data, value.a); + wpi::PackStruct<4>(data, value.b); + } +}; + +template <> +struct wpi::Struct { + static constexpr std::string_view GetTypeString() { return "struct:Outer"; } + static constexpr size_t GetSize() { return wpi::GetStructSize() + 4; } + static constexpr std::string_view GetSchema() { + return "Inner inner; int32 c"; + } + + static Outer Unpack(std::span data) { + constexpr size_t innerSize = wpi::GetStructSize(); + return {wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}; + } + static void Pack(std::span data, const Outer& value) { + constexpr size_t innerSize = wpi::GetStructSize(); + wpi::PackStruct<0>(data, value.inner); + wpi::PackStruct(data, value.c); + } + static void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + } +}; + +template <> +struct wpi::Struct { + static std::string_view GetTypeString() { return "struct:Inner2"; } + static size_t GetSize() { return 8; } + static std::string_view GetSchema() { return "int32 a; int32 b"; } + + static Inner2 Unpack(std::span data) { + return {wpi::UnpackStruct(data), + wpi::UnpackStruct(data)}; + } + static void Pack(std::span data, const Inner2& value) { + wpi::PackStruct<0>(data, value.a); + wpi::PackStruct<4>(data, value.b); + } +}; + +template <> +struct wpi::Struct { + static std::string_view GetTypeString() { return "struct:Outer2"; } + static size_t GetSize() { return wpi::GetStructSize() + 4; } + static std::string_view GetSchema() { return "Inner2 inner; int32 c"; } + + static Outer2 Unpack(std::span data) { + size_t innerSize = wpi::GetStructSize(); + return {wpi::UnpackStruct(data), + wpi::UnpackStruct(data.subspan(innerSize))}; + } + static void Pack(std::span data, const Outer2& value) { + size_t innerSize = wpi::GetStructSize(); + wpi::PackStruct<0>(data, value.inner); + wpi::PackStruct(data.subspan(innerSize), value.c); + } + static void ForEachNested( + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + } +}; + +namespace nt { + +class StructTest : public ::testing::Test { + public: + StructTest() { inst = nt::NetworkTableInstance::Create(); } + ~StructTest() { nt::NetworkTableInstance::Destroy(inst); } + + nt::NetworkTableInstance inst; +}; + +TEST_F(StructTest, InnerConstexpr) { + nt::StructTopic topic = inst.GetStructTopic("inner"); + nt::StructPublisher pub = topic.Publish(); + nt::StructSubscriber sub = topic.Subscribe({}); + + ASSERT_EQ(topic.GetTypeString(), "struct:Inner"); + + pub.SetDefault({0, 1}); + Inner val = sub.Get(); + ASSERT_EQ(val.a, 0); + ASSERT_EQ(val.b, 1); + + pub.Set({1, 2}); + auto atomicVal = sub.GetAtomic(); + ASSERT_EQ(atomicVal.value.a, 1); + ASSERT_EQ(atomicVal.value.b, 2); + + Inner val2; + sub.GetInto(&val2); + ASSERT_EQ(val2.a, 1); + ASSERT_EQ(val2.b, 2); + + auto vals = sub.ReadQueue(); + ASSERT_EQ(vals.size(), 1u); + ASSERT_EQ(vals[0].value.a, 1); + ASSERT_EQ(vals[0].value.b, 2); +} + +TEST_F(StructTest, InnerNonconstexpr) { + nt::StructTopic topic = inst.GetStructTopic("inner2"); + nt::StructPublisher pub = topic.Publish(); + nt::StructSubscriber sub = topic.Subscribe({}); + + ASSERT_EQ(topic.GetTypeString(), "struct:Inner2"); + + pub.SetDefault({0, 1}); + Inner2 val = sub.Get(); + ASSERT_EQ(val.a, 0); + ASSERT_EQ(val.b, 1); + + pub.Set({1, 2}); + auto atomicVal = sub.GetAtomic(); + ASSERT_EQ(atomicVal.value.a, 1); + ASSERT_EQ(atomicVal.value.b, 2); + + Inner2 val2; + sub.GetInto(&val2); + ASSERT_EQ(val2.a, 1); + ASSERT_EQ(val2.b, 2); + + auto vals = sub.ReadQueue(); + ASSERT_EQ(vals.size(), 1u); + ASSERT_EQ(vals[0].value.a, 1); + ASSERT_EQ(vals[0].value.b, 2); +} + +TEST_F(StructTest, OuterConstexpr) { + nt::StructTopic topic = inst.GetStructTopic("outer"); + nt::StructPublisher pub = topic.Publish(); + nt::StructSubscriber sub = topic.Subscribe({}); + + ASSERT_EQ(topic.GetTypeString(), "struct:Outer"); + + pub.SetDefault({{0, 1}, 2}); + Outer val = sub.Get(); + ASSERT_EQ(val.inner.a, 0); + ASSERT_EQ(val.inner.b, 1); + ASSERT_EQ(val.c, 2); + + pub.Set({{1, 2}, 3}); + auto atomicVal = sub.GetAtomic(); + ASSERT_EQ(atomicVal.value.inner.a, 1); + ASSERT_EQ(atomicVal.value.inner.b, 2); + ASSERT_EQ(atomicVal.value.c, 3); + + Outer val2; + sub.GetInto(&val2); + ASSERT_EQ(val2.inner.a, 1); + ASSERT_EQ(val2.inner.b, 2); + ASSERT_EQ(val2.c, 3); + + auto vals = sub.ReadQueue(); + ASSERT_EQ(vals.size(), 1u); + ASSERT_EQ(vals[0].value.inner.a, 1); + ASSERT_EQ(vals[0].value.inner.b, 2); + ASSERT_EQ(vals[0].value.c, 3); +} + +TEST_F(StructTest, OuterNonconstexpr) { + nt::StructTopic topic = inst.GetStructTopic("outer2"); + nt::StructPublisher pub = topic.Publish(); + nt::StructSubscriber sub = topic.Subscribe({}); + + ASSERT_EQ(topic.GetTypeString(), "struct:Outer2"); + + pub.SetDefault({{0, 1}, 2}); + Outer2 val = sub.Get(); + ASSERT_EQ(val.inner.a, 0); + ASSERT_EQ(val.inner.b, 1); + ASSERT_EQ(val.c, 2); + + pub.Set({{1, 2}, 3}); + auto atomicVal = sub.GetAtomic(); + ASSERT_EQ(atomicVal.value.inner.a, 1); + ASSERT_EQ(atomicVal.value.inner.b, 2); + ASSERT_EQ(atomicVal.value.c, 3); + + Outer2 val2; + sub.GetInto(&val2); + ASSERT_EQ(val2.inner.a, 1); + ASSERT_EQ(val2.inner.b, 2); + ASSERT_EQ(val2.c, 3); + + auto vals = sub.ReadQueue(); + ASSERT_EQ(vals.size(), 1u); + ASSERT_EQ(vals[0].value.inner.a, 1); + ASSERT_EQ(vals[0].value.inner.b, 2); + ASSERT_EQ(vals[0].value.c, 3); +} + +TEST_F(StructTest, InnerArrayConstexpr) { + nt::StructArrayTopic topic = inst.GetStructArrayTopic("innerA"); + nt::StructArrayPublisher pub = topic.Publish(); + nt::StructArraySubscriber sub = topic.Subscribe({}); + + ASSERT_EQ(topic.GetTypeString(), "struct:Inner[]"); + + pub.SetDefault({{{0, 1}}}); + auto val = sub.Get(); + ASSERT_EQ(val.size(), 1u); + ASSERT_EQ(val[0].a, 0); + ASSERT_EQ(val[0].b, 1); + + pub.Set({{{1, 2}}}); + auto atomicVal = sub.GetAtomic(); + ASSERT_EQ(atomicVal.value.size(), 1u); + ASSERT_EQ(atomicVal.value[0].a, 1); + ASSERT_EQ(atomicVal.value[0].b, 2); + + auto vals = sub.ReadQueue(); + ASSERT_EQ(vals.size(), 1u); + ASSERT_EQ(vals[0].value.size(), 1u); + ASSERT_EQ(vals[0].value[0].a, 1); + ASSERT_EQ(vals[0].value[0].b, 2); +} + +TEST_F(StructTest, InnerArrayNonconstexpr) { + nt::StructArrayTopic topic = + inst.GetStructArrayTopic("innerA2"); + nt::StructArrayPublisher pub = topic.Publish(); + nt::StructArraySubscriber sub = topic.Subscribe({}); + + ASSERT_EQ(topic.GetTypeString(), "struct:Inner2[]"); + + pub.SetDefault({{{0, 1}}}); + auto val = sub.Get(); + ASSERT_EQ(val.size(), 1u); + ASSERT_EQ(val[0].a, 0); + ASSERT_EQ(val[0].b, 1); + + pub.Set({{{1, 2}}}); + auto atomicVal = sub.GetAtomic(); + ASSERT_EQ(atomicVal.value.size(), 1u); + ASSERT_EQ(atomicVal.value[0].a, 1); + ASSERT_EQ(atomicVal.value[0].b, 2); + + auto vals = sub.ReadQueue(); + ASSERT_EQ(vals.size(), 1u); + ASSERT_EQ(vals[0].value.size(), 1u); + ASSERT_EQ(vals[0].value[0].a, 1); + ASSERT_EQ(vals[0].value[0].b, 2); +} + +} // namespace nt diff --git a/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp index 68e63476ac..b1bb9aef52 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp @@ -7,26 +7,19 @@ namespace { constexpr size_t kTranslationOff = 0; constexpr size_t kRotationOff = - kTranslationOff + wpi::Struct::kSize; + kTranslationOff + wpi::GetStructSize(); } // namespace using StructType = wpi::Struct; -frc::Pose2d StructType::Unpack(std::span data) { +frc::Pose2d StructType::Unpack(std::span data) { return frc::Pose2d{ wpi::UnpackStruct(data), wpi::UnpackStruct(data), }; } -void StructType::Pack(std::span data, - const frc::Pose2d& value) { +void StructType::Pack(std::span data, const frc::Pose2d& value) { wpi::PackStruct(data, value.Translation()); wpi::PackStruct(data, value.Rotation()); } - -void StructType::ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); -} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp index 37995781e1..104a51b935 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp @@ -7,26 +7,19 @@ namespace { constexpr size_t kTranslationOff = 0; constexpr size_t kRotationOff = - kTranslationOff + wpi::Struct::kSize; + kTranslationOff + wpi::GetStructSize(); } // namespace using StructType = wpi::Struct; -frc::Pose3d StructType::Unpack(std::span data) { +frc::Pose3d StructType::Unpack(std::span data) { return frc::Pose3d{ wpi::UnpackStruct(data), wpi::UnpackStruct(data), }; } -void StructType::Pack(std::span data, - const frc::Pose3d& value) { +void StructType::Pack(std::span data, const frc::Pose3d& value) { wpi::PackStruct(data, value.Translation()); wpi::PackStruct(data, value.Rotation()); } - -void StructType::ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); -} diff --git a/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp index 140b6faca6..df5b7815e4 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp @@ -13,7 +13,7 @@ constexpr size_t kZOff = kYOff + 8; using StructType = wpi::Struct; -frc::Quaternion StructType::Unpack(std::span data) { +frc::Quaternion StructType::Unpack(std::span data) { return frc::Quaternion{ wpi::UnpackStruct(data), wpi::UnpackStruct(data), @@ -22,8 +22,7 @@ frc::Quaternion StructType::Unpack(std::span data) { }; } -void StructType::Pack(std::span data, - const frc::Quaternion& value) { +void StructType::Pack(std::span data, const frc::Quaternion& value) { wpi::PackStruct(data, value.W()); wpi::PackStruct(data, value.X()); wpi::PackStruct(data, value.Y()); diff --git a/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp index 8027858cb2..16d3b40d18 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp @@ -10,13 +10,12 @@ constexpr size_t kValueOff = 0; using StructType = wpi::Struct; -frc::Rotation2d StructType::Unpack(std::span data) { +frc::Rotation2d StructType::Unpack(std::span data) { return frc::Rotation2d{ units::radian_t{wpi::UnpackStruct(data)}, }; } -void StructType::Pack(std::span data, - const frc::Rotation2d& value) { +void StructType::Pack(std::span data, const frc::Rotation2d& value) { wpi::PackStruct(data, value.Radians().value()); } diff --git a/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp index e2a122d561..926c7c154a 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp @@ -10,18 +10,12 @@ constexpr size_t kQOff = 0; using StructType = wpi::Struct; -frc::Rotation3d StructType::Unpack(std::span data) { +frc::Rotation3d StructType::Unpack(std::span data) { return frc::Rotation3d{ wpi::UnpackStruct(data), }; } -void StructType::Pack(std::span data, - const frc::Rotation3d& value) { +void StructType::Pack(std::span data, const frc::Rotation3d& value) { wpi::PackStruct(data, value.GetQuaternion()); } - -void StructType::ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); -} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp index 25ef5119a0..35fa6bf8f3 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp @@ -7,26 +7,19 @@ namespace { constexpr size_t kTranslationOff = 0; constexpr size_t kRotationOff = - kTranslationOff + wpi::Struct::kSize; + kTranslationOff + wpi::GetStructSize(); } // namespace using StructType = wpi::Struct; -frc::Transform2d StructType::Unpack(std::span data) { +frc::Transform2d StructType::Unpack(std::span data) { return frc::Transform2d{ wpi::UnpackStruct(data), wpi::UnpackStruct(data), }; } -void StructType::Pack(std::span data, - const frc::Transform2d& value) { +void StructType::Pack(std::span data, const frc::Transform2d& value) { wpi::PackStruct(data, value.Translation()); wpi::PackStruct(data, value.Rotation()); } - -void StructType::ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); -} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp index 5f169c5b2a..b428346df5 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp @@ -7,26 +7,19 @@ namespace { constexpr size_t kTranslationOff = 0; constexpr size_t kRotationOff = - kTranslationOff + wpi::Struct::kSize; + kTranslationOff + wpi::GetStructSize(); } // namespace using StructType = wpi::Struct; -frc::Transform3d StructType::Unpack(std::span data) { +frc::Transform3d StructType::Unpack(std::span data) { return frc::Transform3d{ wpi::UnpackStruct(data), wpi::UnpackStruct(data), }; } -void StructType::Pack(std::span data, - const frc::Transform3d& value) { +void StructType::Pack(std::span data, const frc::Transform3d& value) { wpi::PackStruct(data, value.Translation()); wpi::PackStruct(data, value.Rotation()); } - -void StructType::ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); - wpi::ForEachStructSchema(fn); -} diff --git a/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp index e9deb8ea29..edf2574b9a 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp @@ -11,14 +11,14 @@ constexpr size_t kYOff = kXOff + 8; using StructType = wpi::Struct; -frc::Translation2d StructType::Unpack(std::span data) { +frc::Translation2d StructType::Unpack(std::span data) { return frc::Translation2d{ units::meter_t{wpi::UnpackStruct(data)}, units::meter_t{wpi::UnpackStruct(data)}, }; } -void StructType::Pack(std::span data, +void StructType::Pack(std::span data, const frc::Translation2d& value) { wpi::PackStruct(data, value.X().value()); wpi::PackStruct(data, value.Y().value()); diff --git a/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp index d747cb3c22..f69306c994 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp @@ -12,7 +12,7 @@ constexpr size_t kZOff = kYOff + 8; using StructType = wpi::Struct; -frc::Translation3d StructType::Unpack(std::span data) { +frc::Translation3d StructType::Unpack(std::span data) { return frc::Translation3d{ units::meter_t{wpi::UnpackStruct(data)}, units::meter_t{wpi::UnpackStruct(data)}, @@ -20,7 +20,7 @@ frc::Translation3d StructType::Unpack(std::span data) { }; } -void StructType::Pack(std::span data, +void StructType::Pack(std::span data, const frc::Translation3d& value) { wpi::PackStruct(data, value.X().value()); wpi::PackStruct(data, value.Y().value()); diff --git a/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp index 9ad58199e4..5c71c62018 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp @@ -12,7 +12,7 @@ constexpr size_t kDthetaOff = kDyOff + 8; using StructType = wpi::Struct; -frc::Twist2d StructType::Unpack(std::span data) { +frc::Twist2d StructType::Unpack(std::span data) { return frc::Twist2d{ units::meter_t{wpi::UnpackStruct(data)}, units::meter_t{wpi::UnpackStruct(data)}, @@ -20,8 +20,7 @@ frc::Twist2d StructType::Unpack(std::span data) { }; } -void StructType::Pack(std::span data, - const frc::Twist2d& value) { +void StructType::Pack(std::span data, const frc::Twist2d& value) { wpi::PackStruct(data, value.dx.value()); wpi::PackStruct(data, value.dy.value()); wpi::PackStruct(data, value.dtheta.value()); diff --git a/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp index f434a8f1a9..d933d4b394 100644 --- a/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp +++ b/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp @@ -15,7 +15,7 @@ constexpr size_t kRzOff = kRyOff + 8; using StructType = wpi::Struct; -frc::Twist3d StructType::Unpack(std::span data) { +frc::Twist3d StructType::Unpack(std::span data) { return frc::Twist3d{ units::meter_t{wpi::UnpackStruct(data)}, units::meter_t{wpi::UnpackStruct(data)}, @@ -26,8 +26,7 @@ frc::Twist3d StructType::Unpack(std::span data) { }; } -void StructType::Pack(std::span data, - const frc::Twist3d& value) { +void StructType::Pack(std::span data, const frc::Twist3d& value) { wpi::PackStruct(data, value.dx.value()); wpi::PackStruct(data, value.dy.value()); wpi::PackStruct(data, value.dz.value()); diff --git a/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp index cb17a0c86d..3b9ec2bd3c 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp @@ -12,7 +12,7 @@ constexpr size_t kOmegaOff = kVyOff + 8; using StructType = wpi::Struct; -frc::ChassisSpeeds StructType::Unpack(std::span data) { +frc::ChassisSpeeds StructType::Unpack(std::span data) { return frc::ChassisSpeeds{ units::meters_per_second_t{wpi::UnpackStruct(data)}, units::meters_per_second_t{wpi::UnpackStruct(data)}, @@ -20,7 +20,7 @@ frc::ChassisSpeeds StructType::Unpack(std::span data) { }; } -void StructType::Pack(std::span data, +void StructType::Pack(std::span data, const frc::ChassisSpeeds& value) { wpi::PackStruct(data, value.vx.value()); wpi::PackStruct(data, value.vy.value()); diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp index 9ce43860f1..b7be0d2266 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp @@ -11,13 +11,13 @@ constexpr size_t kTrackWidthOff = 0; using StructType = wpi::Struct; frc::DifferentialDriveKinematics StructType::Unpack( - std::span data) { + std::span data) { return frc::DifferentialDriveKinematics{ units::meter_t{wpi::UnpackStruct(data)}, }; } -void StructType::Pack(std::span data, +void StructType::Pack(std::span data, const frc::DifferentialDriveKinematics& value) { wpi::PackStruct(data, value.trackWidth.value()); } diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp index 35c157c83d..e2ae13131c 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp @@ -12,14 +12,14 @@ constexpr size_t kRightOff = kLeftOff + 8; using StructType = wpi::Struct; frc::DifferentialDriveWheelSpeeds StructType::Unpack( - std::span data) { + std::span data) { return frc::DifferentialDriveWheelSpeeds{ units::meters_per_second_t{wpi::UnpackStruct(data)}, units::meters_per_second_t{wpi::UnpackStruct(data)}, }; } -void StructType::Pack(std::span data, +void StructType::Pack(std::span data, const frc::DifferentialDriveWheelSpeeds& value) { wpi::PackStruct(data, value.left.value()); wpi::PackStruct(data, value.right.value()); diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp index 135618e5ad..261966cee1 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp @@ -7,17 +7,16 @@ namespace { constexpr size_t kFrontLeftOff = 0; constexpr size_t kFrontRightOff = - kFrontLeftOff + wpi::Struct::kSize; + kFrontLeftOff + wpi::GetStructSize(); constexpr size_t kRearLeftOff = - kFrontRightOff + wpi::Struct::kSize; + kFrontRightOff + wpi::GetStructSize(); constexpr size_t kRearRightOff = - kRearLeftOff + wpi::Struct::kSize; + kRearLeftOff + wpi::GetStructSize(); } // namespace using StructType = wpi::Struct; -frc::MecanumDriveKinematics StructType::Unpack( - std::span data) { +frc::MecanumDriveKinematics StructType::Unpack(std::span data) { return frc::MecanumDriveKinematics{ wpi::UnpackStruct(data), wpi::UnpackStruct(data), @@ -26,15 +25,10 @@ frc::MecanumDriveKinematics StructType::Unpack( }; } -void StructType::Pack(std::span data, +void StructType::Pack(std::span data, const frc::MecanumDriveKinematics& value) { wpi::PackStruct(data, value.GetFrontLeftWheel()); wpi::PackStruct(data, value.GetFrontRightWheel()); wpi::PackStruct(data, value.GetRearLeftWheel()); wpi::PackStruct(data, value.GetRearRightWheel()); } - -void StructType::ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); -} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp index d75df6e7dc..c857f4de52 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp @@ -14,7 +14,7 @@ constexpr size_t kRearRightOff = kRearLeftOff + 8; using StructType = wpi::Struct; frc::MecanumDriveWheelPositions StructType::Unpack( - std::span data) { + std::span data) { return frc::MecanumDriveWheelPositions{ units::meter_t{wpi::UnpackStruct(data)}, units::meter_t{wpi::UnpackStruct(data)}, @@ -23,7 +23,7 @@ frc::MecanumDriveWheelPositions StructType::Unpack( }; } -void StructType::Pack(std::span data, +void StructType::Pack(std::span data, const frc::MecanumDriveWheelPositions& value) { wpi::PackStruct(data, value.frontLeft.value()); wpi::PackStruct(data, value.frontRight.value()); diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp index ef173f4f64..c232cc9828 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp @@ -13,8 +13,7 @@ constexpr size_t kRearRightOff = kRearLeftOff + 8; using StructType = wpi::Struct; -frc::MecanumDriveWheelSpeeds StructType::Unpack( - std::span data) { +frc::MecanumDriveWheelSpeeds StructType::Unpack(std::span data) { return frc::MecanumDriveWheelSpeeds{ units::meters_per_second_t{ wpi::UnpackStruct(data)}, @@ -26,7 +25,7 @@ frc::MecanumDriveWheelSpeeds StructType::Unpack( }; } -void StructType::Pack(std::span data, +void StructType::Pack(std::span data, const frc::MecanumDriveWheelSpeeds& value) { wpi::PackStruct(data, value.frontLeft.value()); wpi::PackStruct(data, value.frontRight.value()); diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp index 56de1ed169..12ae0a111c 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp @@ -11,21 +11,15 @@ constexpr size_t kAngleOff = kDistanceOff + 8; using StructType = wpi::Struct; -frc::SwerveModulePosition StructType::Unpack( - std::span data) { +frc::SwerveModulePosition StructType::Unpack(std::span data) { return frc::SwerveModulePosition{ units::meter_t{wpi::UnpackStruct(data)}, wpi::UnpackStruct(data), }; } -void StructType::Pack(std::span data, +void StructType::Pack(std::span data, const frc::SwerveModulePosition& value) { wpi::PackStruct(data, value.distance.value()); wpi::PackStruct(data, value.angle); } - -void StructType::ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); -} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp index 396b223f18..b045178c10 100644 --- a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp +++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp @@ -11,21 +11,15 @@ constexpr size_t kAngleOff = kSpeedOff + 8; using StructType = wpi::Struct; -frc::SwerveModuleState StructType::Unpack( - std::span data) { +frc::SwerveModuleState StructType::Unpack(std::span data) { return frc::SwerveModuleState{ units::meters_per_second_t{wpi::UnpackStruct(data)}, wpi::UnpackStruct(data), }; } -void StructType::Pack(std::span data, +void StructType::Pack(std::span data, const frc::SwerveModuleState& value) { wpi::PackStruct(data, value.speed.value()); wpi::PackStruct(data, value.angle); } - -void StructType::ForEachNested( - std::invocable auto fn) { - wpi::ForEachStructSchema(fn); -} diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h index ee517fb491..a76eed546d 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Pose2dStruct.h @@ -11,16 +11,22 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Pose2d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation2d translation;Rotation2d rotation"; + static constexpr std::string_view GetTypeString() { return "struct:Pose2d"; } + static constexpr size_t GetSize() { + return wpi::GetStructSize() + + wpi::GetStructSize(); + } + static constexpr std::string_view GetSchema() { + return "Translation2d translation;Rotation2d rotation"; + } - static frc::Pose2d Unpack(std::span data); - static void Pack(std::span data, const frc::Pose2d& value); + static frc::Pose2d Unpack(std::span data); + static void Pack(std::span data, const frc::Pose2d& value); static void ForEachNested( - std::invocable auto fn); + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + } }; static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h index 2f899491a7..e3c2eb489e 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Pose3dStruct.h @@ -11,16 +11,22 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Pose3d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation3d translation;Rotation3d rotation"; + static constexpr std::string_view GetTypeString() { return "struct:Pose3d"; } + static constexpr size_t GetSize() { + return wpi::GetStructSize() + + wpi::GetStructSize(); + } + static constexpr std::string_view GetSchema() { + return "Translation3d translation;Rotation3d rotation"; + } - static frc::Pose3d Unpack(std::span data); - static void Pack(std::span data, const frc::Pose3d& value); + static frc::Pose3d Unpack(std::span data); + static void Pack(std::span data, const frc::Pose3d& value); static void ForEachNested( - std::invocable auto fn); + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + } }; static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h index 35acd4ba35..06410325fb 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/QuaternionStruct.h @@ -11,12 +11,14 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - 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 constexpr std::string_view GetTypeString() { + return "struct:Quaternion"; + } + static constexpr size_t GetSize() { return 32; } + static constexpr std::string_view GetSchema() { + return "double w;double x;double y;double z"; + } - static frc::Quaternion Unpack(std::span data); - static void Pack(std::span data, - const frc::Quaternion& value); + static frc::Quaternion Unpack(std::span data); + static void Pack(std::span data, const frc::Quaternion& value); }; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h index ce4f7a6f02..127a2027af 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Rotation2dStruct.h @@ -11,11 +11,12 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Rotation2d"; - static constexpr size_t kSize = 8; - static constexpr std::string_view kSchema = "double value"; + static constexpr std::string_view GetTypeString() { + return "struct:Rotation2d"; + } + static constexpr size_t GetSize() { return 8; } + static constexpr std::string_view GetSchema() { return "double value"; } - static frc::Rotation2d Unpack(std::span data); - static void Pack(std::span data, - const frc::Rotation2d& value); + static frc::Rotation2d Unpack(std::span data); + static void Pack(std::span data, const frc::Rotation2d& value); }; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h index 9d73f72d40..d1cc1f0ca8 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Rotation3dStruct.h @@ -11,15 +11,20 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Rotation3d"; - static constexpr size_t kSize = wpi::Struct::kSize; - static constexpr std::string_view kSchema = "Quaternion q"; + static constexpr std::string_view GetTypeString() { + return "struct:Rotation3d"; + } + static constexpr size_t GetSize() { + return wpi::GetStructSize(); + } + static constexpr std::string_view GetSchema() { return "Quaternion q"; } - static frc::Rotation3d Unpack(std::span data); - static void Pack(std::span data, - const frc::Rotation3d& value); + static frc::Rotation3d Unpack(std::span data); + static void Pack(std::span data, const frc::Rotation3d& value); static void ForEachNested( - std::invocable auto fn); + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + } }; static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h index 55f0ef8604..36438b577b 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Transform2dStruct.h @@ -11,17 +11,24 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Transform2d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation2d translation;Rotation2d rotation"; + static constexpr std::string_view GetTypeString() { + return "struct:Transform2d"; + } + static constexpr size_t GetSize() { + return wpi::GetStructSize() + + wpi::GetStructSize(); + } + static constexpr std::string_view GetSchema() { + return "Translation2d translation;Rotation2d rotation"; + } - static frc::Transform2d Unpack(std::span data); - static void Pack(std::span data, - const frc::Transform2d& value); + static frc::Transform2d Unpack(std::span data); + static void Pack(std::span data, const frc::Transform2d& value); static void ForEachNested( - std::invocable auto fn); + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + } }; static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h index 4fb592016c..f4de9cafad 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Transform3dStruct.h @@ -11,17 +11,24 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = "struct:Transform3d"; - static constexpr size_t kSize = wpi::Struct::kSize + - wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation3d translation;Rotation3d rotation"; + static constexpr std::string_view GetTypeString() { + return "struct:Transform3d"; + } + static constexpr size_t GetSize() { + return wpi::GetStructSize() + + wpi::GetStructSize(); + } + static constexpr std::string_view GetSchema() { + return "Translation3d translation;Rotation3d rotation"; + } - static frc::Transform3d Unpack(std::span data); - static void Pack(std::span data, - const frc::Transform3d& value); + static frc::Transform3d Unpack(std::span data); + static void Pack(std::span data, const frc::Transform3d& value); static void ForEachNested( - std::invocable auto fn); + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + wpi::ForEachStructSchema(fn); + } }; static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h index 1f6ea39772..a1c77e9003 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Translation2dStruct.h @@ -11,11 +11,12 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - 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 constexpr std::string_view GetTypeString() { + return "struct:Translation2d"; + } + static constexpr size_t GetSize() { return 16; } + static constexpr std::string_view GetSchema() { return "double x;double y"; } - static frc::Translation2d Unpack(std::span data); - static void Pack(std::span data, - const frc::Translation2d& value); + static frc::Translation2d Unpack(std::span data); + static void Pack(std::span data, const frc::Translation2d& value); }; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h index 34604c6e1f..255e9ac900 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Translation3dStruct.h @@ -11,11 +11,14 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - 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 constexpr std::string_view GetTypeString() { + return "struct:Translation3d"; + } + static constexpr size_t GetSize() { return 24; } + static constexpr std::string_view GetSchema() { + return "double x;double y;double z"; + } - static frc::Translation3d Unpack(std::span data); - static void Pack(std::span data, - const frc::Translation3d& value); + static frc::Translation3d Unpack(std::span data); + static void Pack(std::span data, const frc::Translation3d& value); }; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h index bda746c6da..adff1294b2 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Twist2dStruct.h @@ -11,11 +11,12 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - 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 constexpr std::string_view GetTypeString() { return "struct:Twist2d"; } + static constexpr size_t GetSize() { return 24; } + static constexpr std::string_view GetSchema() { + return "double dx;double dy;double dtheta"; + } - static frc::Twist2d Unpack(std::span data); - static void Pack(std::span data, const frc::Twist2d& value); + static frc::Twist2d Unpack(std::span data); + static void Pack(std::span data, const frc::Twist2d& value); }; diff --git a/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h b/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h index 7cbe8a5d20..64ca8abd3d 100644 --- a/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h +++ b/wpimath/src/main/native/include/frc/geometry/struct/Twist3dStruct.h @@ -11,11 +11,12 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - 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 constexpr std::string_view GetTypeString() { return "struct:Twist3d"; } + static constexpr size_t GetSize() { return 48; } + static constexpr std::string_view GetSchema() { + return "double dx;double dy;double dz;double rx;double ry;double rz"; + } - static frc::Twist3d Unpack(std::span data); - static void Pack(std::span data, const frc::Twist3d& value); + static frc::Twist3d Unpack(std::span data); + static void Pack(std::span data, const frc::Twist3d& value); }; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h index 4e6547bd26..e8b6395910 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/ChassisSpeedsStruct.h @@ -11,12 +11,14 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = "struct:ChassisSpeeds"; - static constexpr size_t kSize = 24; - static constexpr std::string_view kSchema = - "double vx;double vy;double omega"; + static constexpr std::string_view GetTypeString() { + return "struct:ChassisSpeeds"; + } + static constexpr size_t GetSize() { return 24; } + static constexpr std::string_view GetSchema() { + return "double vx;double vy;double omega"; + } - static frc::ChassisSpeeds Unpack(std::span data); - static void Pack(std::span data, - const frc::ChassisSpeeds& value); + static frc::ChassisSpeeds Unpack(std::span data); + static void Pack(std::span data, const frc::ChassisSpeeds& value); }; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h index ab1e2a8964..79b44f65fb 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveKinematicsStruct.h @@ -11,13 +11,13 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = - "struct:DifferentialDriveKinematics"; - static constexpr size_t kSize = 8; - static constexpr std::string_view kSchema = "double track_width"; + static constexpr std::string_view GetTypeString() { + return "struct:DifferentialDriveKinematics"; + } + static constexpr size_t GetSize() { return 8; } + static constexpr std::string_view GetSchema() { return "double track_width"; } - static frc::DifferentialDriveKinematics Unpack( - std::span data); - static void Pack(std::span data, + static frc::DifferentialDriveKinematics Unpack(std::span data); + static void Pack(std::span data, const frc::DifferentialDriveKinematics& value); }; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h index 3810160385..0680020604 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h @@ -11,13 +11,16 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = - "struct:DifferentialDriveWheelSpeeds"; - static constexpr size_t kSize = 16; - static constexpr std::string_view kSchema = "double left;double right"; + static constexpr std::string_view GetTypeString() { + return "struct:DifferentialDriveWheelSpeeds"; + } + static constexpr size_t GetSize() { return 16; } + static constexpr std::string_view GetSchema() { + return "double left;double right"; + } static frc::DifferentialDriveWheelSpeeds Unpack( - std::span data); - static void Pack(std::span data, + std::span data); + static void Pack(std::span data, const frc::DifferentialDriveWheelSpeeds& value); }; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h index 2974c6b1d6..4dde54b1a2 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveKinematicsStruct.h @@ -11,19 +11,24 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = - "struct:MecanumDriveKinematics"; - static constexpr size_t kSize = 4 * wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "Translation2d front_left;Translation2d front_right;Translation2d " - "rear_left;Translation2d rear_right"; + static constexpr std::string_view GetTypeString() { + return "struct:MecanumDriveKinematics"; + } + static constexpr size_t GetSize() { + return 4 * wpi::GetStructSize(); + } + static constexpr std::string_view GetSchema() { + return "Translation2d front_left;Translation2d front_right;Translation2d " + "rear_left;Translation2d rear_right"; + } - static frc::MecanumDriveKinematics Unpack( - std::span data); - static void Pack(std::span data, + static frc::MecanumDriveKinematics Unpack(std::span data); + static void Pack(std::span data, const frc::MecanumDriveKinematics& value); static void ForEachNested( - std::invocable auto fn); + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + } }; static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h index 8fbc3cb316..f14c3749b8 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h @@ -11,14 +11,16 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = - "struct:MecanumDriveWheelPositions"; - static constexpr size_t kSize = 32; - static constexpr std::string_view kSchema = - "double front_left;double front_right;double rear_left;double rear_right"; + static constexpr std::string_view GetTypeString() { + return "struct:MecanumDriveWheelPositions"; + } + static constexpr size_t GetSize() { return 32; } + static constexpr std::string_view GetSchema() { + return "double front_left;double front_right;double rear_left;double " + "rear_right"; + } - static frc::MecanumDriveWheelPositions Unpack( - std::span data); - static void Pack(std::span data, + static frc::MecanumDriveWheelPositions Unpack(std::span data); + static void Pack(std::span data, const frc::MecanumDriveWheelPositions& value); }; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h index f4cfa0987e..b0b0608c19 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h @@ -11,14 +11,16 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = - "struct:MecanumDriveWheelSpeeds"; - static constexpr size_t kSize = 32; - static constexpr std::string_view kSchema = - "double front_left;double front_right;double rear_left;double rear_right"; + static constexpr std::string_view GetTypeString() { + return "struct:MecanumDriveWheelSpeeds"; + } + static constexpr size_t GetSize() { return 32; } + static constexpr std::string_view GetSchema() { + return "double front_left;double front_right;double rear_left;" + "double rear_right"; + } - static frc::MecanumDriveWheelSpeeds Unpack( - std::span data); - static void Pack(std::span data, + static frc::MecanumDriveWheelSpeeds Unpack(std::span data); + static void Pack(std::span data, const frc::MecanumDriveWheelSpeeds& value); }; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h index 20b1ff60cc..ac0f852bf9 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModulePositionStruct.h @@ -11,16 +11,23 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = "struct:SwerveModulePosition"; - static constexpr size_t kSize = 8 + wpi::Struct::kSize; - static constexpr std::string_view kSchema = - "double distance;Rotation2d angle"; + static constexpr std::string_view GetTypeString() { + return "struct:SwerveModulePosition"; + } + static constexpr size_t GetSize() { + return 8 + wpi::GetStructSize(); + } + static constexpr std::string_view GetSchema() { + return "double distance;Rotation2d angle"; + } - static frc::SwerveModulePosition Unpack(std::span data); - static void Pack(std::span data, + static frc::SwerveModulePosition Unpack(std::span data); + static void Pack(std::span data, const frc::SwerveModulePosition& value); static void ForEachNested( - std::invocable auto fn); + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + } }; static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h index fd854cb54d..589eb2a60d 100644 --- a/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h +++ b/wpimath/src/main/native/include/frc/kinematics/struct/SwerveModuleStateStruct.h @@ -11,15 +11,23 @@ template <> struct WPILIB_DLLEXPORT wpi::Struct { - static constexpr std::string_view kTypeString = "struct:SwerveModuleState"; - static constexpr size_t kSize = 8 + wpi::Struct::kSize; - static constexpr std::string_view kSchema = "double speed;Rotation2d angle"; + static constexpr std::string_view GetTypeString() { + return "struct:SwerveModuleState"; + } + static constexpr size_t GetSize() { + return 8 + wpi::GetStructSize(); + } + static constexpr std::string_view GetSchema() { + return "double speed;Rotation2d angle"; + } - static frc::SwerveModuleState Unpack(std::span data); - static void Pack(std::span data, + static frc::SwerveModuleState Unpack(std::span data); + static void Pack(std::span data, const frc::SwerveModuleState& value); static void ForEachNested( - std::invocable auto fn); + std::invocable auto fn) { + wpi::ForEachStructSchema(fn); + } }; static_assert(wpi::HasNestedStruct); diff --git a/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp index 369a6aae07..f8763a6cea 100644 --- a/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/struct/Pose2dStructTest.cpp @@ -16,8 +16,8 @@ const Pose2d kExpectedData{ } // namespace TEST(Pose2dStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); Pose2d unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp index 5eb1bc2d4e..19a503a8de 100644 --- a/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/struct/Pose3dStructTest.cpp @@ -17,8 +17,8 @@ const Pose3d kExpectedData{ } // namespace TEST(Pose3dStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); Pose3d unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp index 6eede3c6a6..d21eaf15f5 100644 --- a/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/struct/QuaternionStructTest.cpp @@ -15,8 +15,8 @@ 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); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); Quaternion unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp index 9007463209..b215c9ef4f 100644 --- a/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/struct/Rotation2dStructTest.cpp @@ -15,8 +15,8 @@ const Rotation2d kExpectedData{Rotation2d{1.91_rad}}; } // namespace TEST(Rotation2dStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); Rotation2d unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp index 992eb45e6c..9d7dd6c97d 100644 --- a/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/struct/Rotation3dStructTest.cpp @@ -16,8 +16,8 @@ const Rotation3d kExpectedData{ } // namespace TEST(Rotation3dStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); Rotation3d unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp index b28aeaabf6..e3841e243e 100644 --- a/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/struct/Transform2dStructTest.cpp @@ -16,8 +16,8 @@ const Transform2d kExpectedData{ } // namespace TEST(Transform2dStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); Transform2d unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp index d32433183b..a98fd7961c 100644 --- a/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/struct/Transform3dStructTest.cpp @@ -17,8 +17,8 @@ const Transform3d kExpectedData{ } // namespace TEST(Transform3dStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); Transform3d unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp index 6f87420cde..bc081dc26e 100644 --- a/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/struct/Translation2dStructTest.cpp @@ -15,8 +15,8 @@ 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); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); Translation2d unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp index 1caa8a327f..41a58ed659 100644 --- a/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/struct/Translation3dStructTest.cpp @@ -15,8 +15,8 @@ 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); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); Translation3d unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp index a3d1370d4b..7bdc8b03f8 100644 --- a/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/struct/Twist2dStructTest.cpp @@ -15,8 +15,8 @@ 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); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); Twist2d unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp b/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp index b753a3b263..eb72b54550 100644 --- a/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/struct/Twist3dStructTest.cpp @@ -16,8 +16,8 @@ const Twist3d kExpectedData{ } // namespace TEST(Twist3dStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); Twist3d unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp index 6729aa0b9d..79f2a35c90 100644 --- a/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp @@ -16,8 +16,8 @@ const ChassisSpeeds kExpectedData{ } // namespace TEST(ChassisSpeedsStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); ChassisSpeeds unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp index ecf10ad5f7..80b4ad565c 100644 --- a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp @@ -16,8 +16,8 @@ const DifferentialDriveKinematics kExpectedData{ } // namespace TEST(DifferentialDriveKinematicsStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); DifferentialDriveKinematics unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp index 33499f5d4e..8635197c76 100644 --- a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp @@ -16,8 +16,8 @@ const DifferentialDriveWheelSpeeds kExpectedData{ } // namespace TEST(DifferentialDriveWheelSpeedsStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); DifferentialDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp index 8fe0817e27..0b3676d63f 100644 --- a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveKinematicsStructTest.cpp @@ -17,8 +17,8 @@ const MecanumDriveKinematics kExpectedData{MecanumDriveKinematics{ } // namespace TEST(MecanumDriveKinematicsStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); MecanumDriveKinematics unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp index 39190a776f..085f69b052 100644 --- a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStructTest.cpp @@ -16,8 +16,8 @@ const MecanumDriveWheelPositions kExpectedData{ } // namespace TEST(MecanumDriveWheelPositionsStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); MecanumDriveWheelPositions unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp index bbde80cdde..83a234961d 100644 --- a/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp @@ -16,8 +16,8 @@ const MecanumDriveWheelSpeeds kExpectedData{ } // namespace TEST(MecanumDriveWheelSpeedsStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); MecanumDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp index a942efe662..2f92bf7449 100644 --- a/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModulePositionStructTest.cpp @@ -16,8 +16,8 @@ const SwerveModulePosition kExpectedData{ } // namespace TEST(SwerveModulePositionStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); SwerveModulePosition unpacked_data = StructType::Unpack(buffer); diff --git a/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp index 40ca305b8e..5edc99c450 100644 --- a/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp @@ -16,8 +16,8 @@ const SwerveModuleState kExpectedData{ } // namespace TEST(SwerveModuleStateStructTest, Roundtrip) { - uint8_t buffer[StructType::kSize]; - std::memset(buffer, 0, StructType::kSize); + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); StructType::Pack(buffer, kExpectedData); SwerveModuleState unpacked_data = StructType::Unpack(buffer); diff --git a/wpiutil/src/main/native/include/wpi/DataLog.h b/wpiutil/src/main/native/include/wpi/DataLog.h index 99db964628..c78deafce1 100644 --- a/wpiutil/src/main/native/include/wpi/DataLog.h +++ b/wpiutil/src/main/native/include/wpi/DataLog.h @@ -968,9 +968,16 @@ class StructLogEntry : public DataLogEntry { * @param timestamp Time stamp (may be 0 to indicate now) */ void Append(const T& data, int64_t timestamp = 0) { - uint8_t buf[S::kSize]; - S::Pack(buf, data); - m_log->AppendRaw(m_entry, buf, timestamp); + if constexpr (wpi::is_constexpr([] { S::GetSize(); })) { + uint8_t buf[S::GetSize()]; + S::Pack(buf, data); + m_log->AppendRaw(m_entry, buf, timestamp); + } else { + wpi::SmallVector buf; + buf.resize_for_overwrite(S::GetSize()); + S::Pack(buf, data); + m_log->AppendRaw(m_entry, buf, timestamp); + } } }; diff --git a/wpiutil/src/main/native/include/wpi/struct/Struct.h b/wpiutil/src/main/native/include/wpi/struct/Struct.h index 1336707973..530607acda 100644 --- a/wpiutil/src/main/native/include/wpi/struct/Struct.h +++ b/wpiutil/src/main/native/include/wpi/struct/Struct.h @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -14,12 +15,14 @@ #include #include +#include + #include "wpi/Endian.h" -#include "wpi/MathExtras.h" #include "wpi/bit.h" #include "wpi/ct_string.h" #include "wpi/function_ref.h" #include "wpi/mutex.h" +#include "wpi/type_traits.h" namespace wpi { @@ -45,37 +48,39 @@ struct Struct {}; * Implementations must define a template specialization for wpi::Struct with * T being the type that is being serialized/deserialized, with the following * static members (as enforced by this concept): - * - std::string_view kTypeString: the type string - * - size_t kSize: the structure size in bytes - * - std::string_view kSchema: the struct schema + * - std::string_view GetTypeString(): function that returns the type string + * - size_t GetSize(): function that returns the structure size in bytes + * - std::string_view GetSchema(): function that returns the struct schema * - T Unpack(std::span): function for deserialization * - void Pack(std::span, T&& value): function for * serialization * + * If possible, the GetTypeString(), GetSize(), and GetSchema() functions should + * be marked constexpr. GetTypeString() and GetSchema() may return types other + * than std::string_view, as long as the return value is convertible to + * std::string_view. + * * If the struct has nested structs, implementations should also meet the * requirements of HasNestedStruct. */ template -concept StructSerializable = - requires(std::span in, std::span out, T&& value) { - typename Struct>; - { - Struct>::kTypeString - } -> std::convertible_to; - { - Struct>::kSize - } -> std::convertible_to; - { - Struct>::kSchema - } -> std::convertible_to; - { - Struct>::Unpack( - in.subspan<0, Struct>::kSize>()) - } -> std::same_as>; - Struct>::Pack( - out.subspan<0, Struct>::kSize>(), - std::forward(value)); - }; +concept StructSerializable = requires(std::span in, + std::span out, T&& value) { + typename Struct>; + { + Struct>::GetTypeString() + } -> std::convertible_to; + { + Struct>::GetSize() + } -> std::convertible_to; + { + Struct>::GetSchema() + } -> std::convertible_to; + { + Struct>::Unpack(in) + } -> std::same_as>; + Struct>::Pack(out, std::forward(value)); +}; /** * Specifies that a type is capable of in-place raw struct deserialization. @@ -87,8 +92,7 @@ concept StructSerializable = template concept MutableStructSerializable = StructSerializable && requires(T* out, std::span in) { - Struct>::UnpackInto( - out, in.subspan<0, Struct>::kSize>()); + Struct>::UnpackInto(out, in); }; /** @@ -115,7 +119,7 @@ concept HasNestedStruct = * @return Deserialized object */ template -inline T UnpackStruct(std::span::kSize> data) { +inline T UnpackStruct(std::span data) { return Struct::Unpack(data); } @@ -130,7 +134,7 @@ inline T UnpackStruct(std::span::kSize> data) { */ template inline T UnpackStruct(std::span data) { - return Struct::Unpack(data.template subspan::kSize>()); + return Struct::Unpack(data.subspan(Offset)); } /** @@ -140,9 +144,7 @@ inline T UnpackStruct(std::span data) { * @param value object */ template -inline void PackStruct( - std::span>::kSize> data, - T&& value) { +inline void PackStruct(std::span data, T&& value) { Struct>::Pack(data, std::forward(value)); } @@ -156,10 +158,8 @@ inline void PackStruct( */ template inline void PackStruct(std::span data, T&& value) { - Struct>::Pack( - data.template subspan>::kSize>(), - std::forward(value)); + Struct>::Pack(data.subspan(Offset), + std::forward(value)); } /** @@ -169,8 +169,7 @@ inline void PackStruct(std::span data, T&& value) { * @param data raw struct data */ template -inline void UnpackStructInto(T* out, - std::span::kSize> data) { +inline void UnpackStructInto(T* out, std::span data) { if constexpr (MutableStructSerializable) { Struct::UnpackInto(out, data); } else { @@ -190,8 +189,7 @@ inline void UnpackStructInto(T* out, template inline void UnpackStructInto(T* out, std::span data) { if constexpr (MutableStructSerializable) { - Struct::UnpackInto(out, - data.template subspan::kSize>()); + Struct::UnpackInto(out, data.subspan(Offset)); } else { *out = UnpackStruct(data); } @@ -205,59 +203,86 @@ inline void UnpackStructInto(T* out, std::span data) { */ template constexpr auto GetStructTypeString() { - return Struct::kTypeString; + return Struct::GetTypeString(); +} + +/** + * Get the size for a raw struct serializable type + * + * @tparam T serializable type + * @return size + */ +template +constexpr size_t GetStructSize() { + return Struct::GetSize(); } template -consteval auto MakeStructArrayTypeString() { - using namespace literals; - if constexpr (N == std::dynamic_extent) { - return Concat( - ct_string, Struct::kTypeString.size()>{ - Struct::kTypeString}, - "[]"_ct_string); +constexpr auto MakeStructArrayTypeString() { + if constexpr (is_constexpr([] { Struct::GetTypeString(); })) { + constexpr auto typeString = Struct::GetTypeString(); + using namespace literals; + if constexpr (N == std::dynamic_extent) { + return Concat( + ct_string, typeString.size()>{ + typeString}, + "[]"_ct_string); + } else { + return Concat( + ct_string, typeString.size()>{ + typeString}, + "["_ct_string, NumToCtString(), "]"_ct_string); + } } else { - return Concat( - ct_string, Struct::kTypeString.size()>{ - Struct::kTypeString}, - "["_ct_string, NumToCtString(), "]"_ct_string); + if constexpr (N == std::dynamic_extent) { + return fmt::format("{}[]", Struct::GetTypeString()); + } else { + return fmt::format("{}[{}]", Struct::GetTypeString(), N); + } } } template consteval auto MakeStructArraySchema() { - using namespace literals; - if constexpr (N == std::dynamic_extent) { - return Concat( - ct_string, Struct::kSchema.size()>{ - Struct::kSchema}, - "[]"_ct_string); + if constexpr (is_constexpr([] { Struct::GetSchema(); })) { + constexpr auto schema = Struct::GetSchema(); + using namespace literals; + if constexpr (N == std::dynamic_extent) { + return Concat( + ct_string, schema.size()>{schema}, + "[]"_ct_string); + } else { + return Concat( + ct_string, schema.size()>{schema}, + "["_ct_string, NumToCtString(), "]"_ct_string); + } } else { - return Concat( - ct_string, Struct::kSchema.size()>{ - Struct::kSchema}, - "["_ct_string, NumToCtString(), "]"_ct_string); + if constexpr (N == std::dynamic_extent) { + return fmt::format("{}[]", Struct::GetSchema()); + } else { + return fmt::format("{}[{}]", Struct::GetSchema(), N); + } } } template constexpr std::string_view GetStructSchema() { - return Struct::kSchema; + return Struct::GetSchema(); } template constexpr std::span GetStructSchemaBytes() { - return {reinterpret_cast(Struct::kSchema.data()), - Struct::kSchema.size()}; + auto schema = Struct::GetSchema(); + return {reinterpret_cast(schema.data()), schema.size()}; } template void ForEachStructSchema( std::invocable auto fn) { if constexpr (HasNestedStruct) { - Struct::ForEachNested(fn); + Struct>::ForEachNested(fn); } - fn(Struct::kTypeString, Struct::kSchema); + fn(Struct::GetTypeString(), Struct::GetSchema()); } template @@ -282,24 +307,25 @@ class StructArrayBuffer { #endif std::invocable> void Write(U&& data, F&& func) { - if ((std::size(data) * S::kSize) < 256) { + auto size = S::GetSize(); + if ((std::size(data) * size) < 256) { // use the stack uint8_t buf[256]; auto out = buf; for (auto&& val : data) { - S::Pack(std::span{out, out + S::kSize}, + S::Pack(std::span{std::to_address(out), size}, std::forward(val)); - out += S::kSize; + out += size; } func(std::span{buf, out}); } else { std::scoped_lock lock{m_mutex}; - m_buf.resize(std::size(data) * S::kSize); + m_buf.resize(std::size(data) * size); auto out = m_buf.begin(); for (auto&& val : data) { - S::Pack(std::span{out, out + S::kSize}, + S::Pack(std::span{std::to_address(out), size}, std::forward(val)); - out += S::kSize; + out += size; } func(m_buf); } @@ -315,36 +341,38 @@ class StructArrayBuffer { */ template struct Struct> { - static constexpr auto kTypeString = MakeStructArrayTypeString(); - static constexpr size_t kSize = N * Struct::kSize; - static constexpr auto kSchema = MakeStructArraySchema(); - static std::array Unpack(std::span data) { + static constexpr auto GetTypeString() { + return MakeStructArrayTypeString(); + } + static constexpr size_t GetSize() { return N * GetStructSize(); } + static constexpr auto GetSchema() { return MakeStructArraySchema(); } + static std::array Unpack(std::span data) { + auto size = GetStructSize(); std::array result; for (size_t i = 0; i < N; ++i) { result[i] = UnpackStruct(data); - data = data.subspan(Struct::kSize); + data = data.subspan(size); } return result; } - static void Pack(std::span data, - std::span values) { + static void Pack(std::span data, std::span values) { + auto size = GetStructSize(); std::span unsizedData = data; for (auto&& val : values) { - PackStruct<0>(unsizedData, val); - unsizedData = unsizedData.subspan(Struct::kSize); + PackStruct(unsizedData, val); + unsizedData = unsizedData.subspan(size); } } - static void UnpackInto(std::array* out, - std::span data) { + static void UnpackInto(std::array* out, std::span data) { UnpackInto(std::span{*out}, data); } // alternate span-based function - static void UnpackInto(std::span out, - std::span data) { + static void UnpackInto(std::span out, std::span data) { + auto size = GetStructSize(); std::span unsizedData = data; for (size_t i = 0; i < N; ++i) { - UnpackStructInto<0, T>(&out[i], unsizedData); - unsizedData = unsizedData.subspan(Struct::kSize); + UnpackStructInto(&out[i], unsizedData); + unsizedData = unsizedData.subspan(size); } } }; @@ -355,11 +383,11 @@ struct Struct> { */ template <> struct Struct { - static constexpr std::string_view kTypeString = "struct:bool"; - static constexpr size_t kSize = 1; - static constexpr std::string_view kSchema = "bool value"; - static bool Unpack(std::span data) { return data[0]; } - static void Pack(std::span data, bool value) { + static constexpr std::string_view GetTypeString() { return "struct:bool"; } + static constexpr size_t GetSize() { return 1; } + static constexpr std::string_view GetSchema() { return "bool value"; } + static bool Unpack(std::span data) { return data[0]; } + static void Pack(std::span data, bool value) { data[0] = static_cast(value ? 1 : 0); } }; @@ -370,13 +398,11 @@ struct Struct { */ template <> struct Struct { - static constexpr std::string_view kTypeString = "struct:uint8"; - static constexpr size_t kSize = 1; - static constexpr std::string_view kSchema = "uint8 value"; - static uint8_t Unpack(std::span data) { return data[0]; } - static void Pack(std::span data, uint8_t value) { - data[0] = value; - } + static constexpr std::string_view GetTypeString() { return "struct:uint8"; } + static constexpr size_t GetSize() { return 1; } + static constexpr std::string_view GetSchema() { return "uint8 value"; } + static uint8_t Unpack(std::span data) { return data[0]; } + static void Pack(std::span data, uint8_t value) { data[0] = value; } }; /** @@ -400,13 +426,13 @@ struct Struct { */ template <> struct Struct { - static constexpr std::string_view kTypeString = "struct:uint16"; - static constexpr size_t kSize = 2; - static constexpr std::string_view kSchema = "uint16 value"; - static uint16_t Unpack(std::span data) { + static constexpr std::string_view GetTypeString() { return "struct:uint16"; } + static constexpr size_t GetSize() { return 2; } + static constexpr std::string_view GetSchema() { return "uint16 value"; } + static uint16_t Unpack(std::span data) { return support::endian::read16le(data.data()); } - static void Pack(std::span data, uint16_t value) { + static void Pack(std::span data, uint16_t value) { support::endian::write16le(data.data(), value); } }; @@ -417,13 +443,13 @@ struct Struct { */ template <> struct Struct { - static constexpr std::string_view kTypeString = "struct:int16"; - static constexpr size_t kSize = 2; - static constexpr std::string_view kSchema = "int16 value"; - static int16_t Unpack(std::span data) { + static constexpr std::string_view GetTypeString() { return "struct:int16"; } + static constexpr size_t GetSize() { return 2; } + static constexpr std::string_view GetSchema() { return "int16 value"; } + static int16_t Unpack(std::span data) { return support::endian::read16le(data.data()); } - static void Pack(std::span data, int16_t value) { + static void Pack(std::span data, int16_t value) { support::endian::write16le(data.data(), value); } }; @@ -434,13 +460,13 @@ struct Struct { */ template <> struct Struct { - static constexpr std::string_view kTypeString = "struct:uint32"; - static constexpr size_t kSize = 4; - static constexpr std::string_view kSchema = "uint32 value"; - static uint32_t Unpack(std::span data) { + static constexpr std::string_view GetTypeString() { return "struct:uint32"; } + static constexpr size_t GetSize() { return 4; } + static constexpr std::string_view GetSchema() { return "uint32 value"; } + static uint32_t Unpack(std::span data) { return support::endian::read32le(data.data()); } - static void Pack(std::span data, uint32_t value) { + static void Pack(std::span data, uint32_t value) { support::endian::write32le(data.data(), value); } }; @@ -451,13 +477,13 @@ struct Struct { */ template <> struct Struct { - static constexpr std::string_view kTypeString = "struct:int32"; - static constexpr size_t kSize = 4; - static constexpr std::string_view kSchema = "int32 value"; - static int32_t Unpack(std::span data) { + static constexpr std::string_view GetTypeString() { return "struct:int32"; } + static constexpr size_t GetSize() { return 4; } + static constexpr std::string_view GetSchema() { return "int32 value"; } + static int32_t Unpack(std::span data) { return support::endian::read32le(data.data()); } - static void Pack(std::span data, int32_t value) { + static void Pack(std::span data, int32_t value) { support::endian::write32le(data.data(), value); } }; @@ -468,13 +494,13 @@ struct Struct { */ template <> struct Struct { - static constexpr std::string_view kTypeString = "struct:uint64"; - static constexpr size_t kSize = 8; - static constexpr std::string_view kSchema = "uint64 value"; - static uint64_t Unpack(std::span data) { + static constexpr std::string_view GetTypeString() { return "struct:uint64"; } + static constexpr size_t GetSize() { return 8; } + static constexpr std::string_view GetSchema() { return "uint64 value"; } + static uint64_t Unpack(std::span data) { return support::endian::read64le(data.data()); } - static void Pack(std::span data, uint64_t value) { + static void Pack(std::span data, uint64_t value) { support::endian::write64le(data.data(), value); } }; @@ -485,13 +511,13 @@ struct Struct { */ template <> struct Struct { - static constexpr std::string_view kTypeString = "struct:int64"; - static constexpr size_t kSize = 8; - static constexpr std::string_view kSchema = "int64 value"; - static int64_t Unpack(std::span data) { + static constexpr std::string_view GetTypeString() { return "struct:int64"; } + static constexpr size_t GetSize() { return 8; } + static constexpr std::string_view GetSchema() { return "int64 value"; } + static int64_t Unpack(std::span data) { return support::endian::read64le(data.data()); } - static void Pack(std::span data, int64_t value) { + static void Pack(std::span data, int64_t value) { support::endian::write64le(data.data(), value); } }; @@ -502,13 +528,13 @@ struct Struct { */ template <> struct Struct { - static constexpr std::string_view kTypeString = "struct:float"; - static constexpr size_t kSize = 4; - static constexpr std::string_view kSchema = "float value"; - static float Unpack(std::span data) { + static constexpr std::string_view GetTypeString() { return "struct:float"; } + static constexpr size_t GetSize() { return 4; } + static constexpr std::string_view GetSchema() { return "float value"; } + static float Unpack(std::span data) { return bit_cast(support::endian::read32le(data.data())); } - static void Pack(std::span data, float value) { + static void Pack(std::span data, float value) { support::endian::write32le(data.data(), bit_cast(value)); } }; @@ -519,13 +545,13 @@ struct Struct { */ template <> struct Struct { - static constexpr std::string_view kTypeString = "struct:double"; - static constexpr size_t kSize = 8; - static constexpr std::string_view kSchema = "double value"; - static double Unpack(std::span data) { + static constexpr std::string_view GetTypeString() { return "struct:double"; } + static constexpr size_t GetSize() { return 8; } + static constexpr std::string_view GetSchema() { return "double value"; } + static double Unpack(std::span data) { return bit_cast(support::endian::read64le(data.data())); } - static void Pack(std::span data, double value) { + static void Pack(std::span data, double value) { support::endian::write64le(data.data(), bit_cast(value)); } };