[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.
This commit is contained in:
Peter Johnson
2023-12-02 23:36:44 -08:00
committed by GitHub
parent ca272de400
commit a583ca01e1
62 changed files with 812 additions and 450 deletions

View File

@@ -7,9 +7,9 @@
#include <stdint.h>
#include <atomic>
#include <memory>
#include <ranges>
#include <span>
#include <string_view>
#include <utility>
#include <vector>
@@ -121,16 +121,17 @@ class StructArraySubscriber : public Subscriber {
#endif
TimestampedValueType GetAtomic(U&& defaultValue) const {
wpi::SmallVector<uint8_t, 128> 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<U>(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<const uint8_t, S::kSize>{in, in + S::kSize}));
S::Unpack(std::span<const uint8_t>{std::to_address(in), size}));
}
return rv;
}
@@ -145,16 +146,17 @@ class StructArraySubscriber : public Subscriber {
*/
TimestampedValueType GetAtomic(std::span<const T> defaultValue) const {
wpi::SmallVector<uint8_t, 128> 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<const uint8_t, S::kSize>{in, in + S::kSize}));
S::Unpack(std::span<const uint8_t>{std::to_address(in), size}));
}
return rv;
}
@@ -174,16 +176,17 @@ class StructArraySubscriber : public Subscriber {
auto raw = ::nt::ReadQueueRaw(m_subHandle);
std::vector<TimestampedValueType> 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<T> 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<const uint8_t, S::kSize>{in, in + S::kSize}));
S::Unpack(std::span<const uint8_t>{std::to_address(in), size}));
}
rv.emplace_back(r.time, r.serverTime, std::move(values));
}

View File

@@ -32,6 +32,13 @@ class StructTopic;
template <wpi::StructSerializable T>
class StructSubscriber : public Subscriber {
using S = wpi::Struct<T>;
static constexpr size_t kBufSize = []() -> size_t {
if constexpr (wpi::is_constexpr([] { S::GetSize(); })) {
return S::GetSize();
} else {
return 128;
}
}();
public:
using TopicType = StructTopic<T>;
@@ -81,12 +88,12 @@ class StructSubscriber : public Subscriber {
* @return true if successful
*/
bool GetInto(T* out) {
wpi::SmallVector<uint8_t, S::kSize> buf;
wpi::SmallVector<uint8_t, kBufSize> 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<uint8_t, S::kSize> buf;
wpi::SmallVector<uint8_t, kBufSize> 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<TimestampedValueType> 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<const uint8_t>(r.value).subspan<0, S::kSize>()));
rv.emplace_back(r.time, r.serverTime,
S::Unpack(std::span<const uint8_t>(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<T>();
}
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<uint8_t, 128> 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<T>();
}
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<uint8_t, 128> buf;
buf.resize_for_overwrite(S::GetSize());
S::Pack(buf, value);
::nt::SetDefaultRaw(m_pubHandle, buf);
}
}
/**

View File

@@ -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 <gtest/gtest.h>
#include <wpi/SpanMatcher.h>
#include <wpi/struct/Struct.h>
#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<Inner> {
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<const uint8_t> data) {
return {wpi::UnpackStruct<int32_t, 0>(data),
wpi::UnpackStruct<int32_t, 4>(data)};
}
static void Pack(std::span<uint8_t> data, const Inner& value) {
wpi::PackStruct<0>(data, value.a);
wpi::PackStruct<4>(data, value.b);
}
};
template <>
struct wpi::Struct<Outer> {
static constexpr std::string_view GetTypeString() { return "struct:Outer"; }
static constexpr size_t GetSize() { return wpi::GetStructSize<Inner>() + 4; }
static constexpr std::string_view GetSchema() {
return "Inner inner; int32 c";
}
static Outer Unpack(std::span<const uint8_t> data) {
constexpr size_t innerSize = wpi::GetStructSize<Inner>();
return {wpi::UnpackStruct<Inner, 0>(data),
wpi::UnpackStruct<int32_t, innerSize>(data)};
}
static void Pack(std::span<uint8_t> data, const Outer& value) {
constexpr size_t innerSize = wpi::GetStructSize<Inner>();
wpi::PackStruct<0>(data, value.inner);
wpi::PackStruct<innerSize>(data, value.c);
}
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<Inner>(fn);
}
};
template <>
struct wpi::Struct<Inner2> {
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<const uint8_t> data) {
return {wpi::UnpackStruct<int32_t, 0>(data),
wpi::UnpackStruct<int32_t, 4>(data)};
}
static void Pack(std::span<uint8_t> data, const Inner2& value) {
wpi::PackStruct<0>(data, value.a);
wpi::PackStruct<4>(data, value.b);
}
};
template <>
struct wpi::Struct<Outer2> {
static std::string_view GetTypeString() { return "struct:Outer2"; }
static size_t GetSize() { return wpi::GetStructSize<Inner>() + 4; }
static std::string_view GetSchema() { return "Inner2 inner; int32 c"; }
static Outer2 Unpack(std::span<const uint8_t> data) {
size_t innerSize = wpi::GetStructSize<Inner2>();
return {wpi::UnpackStruct<Inner2, 0>(data),
wpi::UnpackStruct<int32_t>(data.subspan(innerSize))};
}
static void Pack(std::span<uint8_t> data, const Outer2& value) {
size_t innerSize = wpi::GetStructSize<Inner2>();
wpi::PackStruct<0>(data, value.inner);
wpi::PackStruct(data.subspan(innerSize), value.c);
}
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<Inner2>(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<Inner> topic = inst.GetStructTopic<Inner>("inner");
nt::StructPublisher<Inner> pub = topic.Publish();
nt::StructSubscriber<Inner> 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<Inner2> topic = inst.GetStructTopic<Inner2>("inner2");
nt::StructPublisher<Inner2> pub = topic.Publish();
nt::StructSubscriber<Inner2> 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<Outer> topic = inst.GetStructTopic<Outer>("outer");
nt::StructPublisher<Outer> pub = topic.Publish();
nt::StructSubscriber<Outer> 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<Outer2> topic = inst.GetStructTopic<Outer2>("outer2");
nt::StructPublisher<Outer2> pub = topic.Publish();
nt::StructSubscriber<Outer2> 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<Inner> topic = inst.GetStructArrayTopic<Inner>("innerA");
nt::StructArrayPublisher<Inner> pub = topic.Publish();
nt::StructArraySubscriber<Inner> 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<Inner2> topic =
inst.GetStructArrayTopic<Inner2>("innerA2");
nt::StructArrayPublisher<Inner2> pub = topic.Publish();
nt::StructArraySubscriber<Inner2> 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

View File

@@ -7,26 +7,19 @@
namespace {
constexpr size_t kTranslationOff = 0;
constexpr size_t kRotationOff =
kTranslationOff + wpi::Struct<frc::Translation2d>::kSize;
kTranslationOff + wpi::GetStructSize<frc::Translation2d>();
} // namespace
using StructType = wpi::Struct<frc::Pose2d>;
frc::Pose2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::Pose2d StructType::Unpack(std::span<const uint8_t> data) {
return frc::Pose2d{
wpi::UnpackStruct<frc::Translation2d, kTranslationOff>(data),
wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Pose2d& value) {
void StructType::Pack(std::span<uint8_t> data, const frc::Pose2d& value) {
wpi::PackStruct<kTranslationOff>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}

View File

@@ -7,26 +7,19 @@
namespace {
constexpr size_t kTranslationOff = 0;
constexpr size_t kRotationOff =
kTranslationOff + wpi::Struct<frc::Translation3d>::kSize;
kTranslationOff + wpi::GetStructSize<frc::Translation3d>();
} // namespace
using StructType = wpi::Struct<frc::Pose3d>;
frc::Pose3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::Pose3d StructType::Unpack(std::span<const uint8_t> data) {
return frc::Pose3d{
wpi::UnpackStruct<frc::Translation3d, kTranslationOff>(data),
wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Pose3d& value) {
void StructType::Pack(std::span<uint8_t> data, const frc::Pose3d& value) {
wpi::PackStruct<kTranslationOff>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation3d>(fn);
wpi::ForEachStructSchema<frc::Rotation3d>(fn);
}

View File

@@ -13,7 +13,7 @@ constexpr size_t kZOff = kYOff + 8;
using StructType = wpi::Struct<frc::Quaternion>;
frc::Quaternion StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::Quaternion StructType::Unpack(std::span<const uint8_t> data) {
return frc::Quaternion{
wpi::UnpackStruct<double, kWOff>(data),
wpi::UnpackStruct<double, kXOff>(data),
@@ -22,8 +22,7 @@ frc::Quaternion StructType::Unpack(std::span<const uint8_t, kSize> data) {
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Quaternion& value) {
void StructType::Pack(std::span<uint8_t> data, const frc::Quaternion& value) {
wpi::PackStruct<kWOff>(data, value.W());
wpi::PackStruct<kXOff>(data, value.X());
wpi::PackStruct<kYOff>(data, value.Y());

View File

@@ -10,13 +10,12 @@ constexpr size_t kValueOff = 0;
using StructType = wpi::Struct<frc::Rotation2d>;
frc::Rotation2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::Rotation2d StructType::Unpack(std::span<const uint8_t> data) {
return frc::Rotation2d{
units::radian_t{wpi::UnpackStruct<double, kValueOff>(data)},
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Rotation2d& value) {
void StructType::Pack(std::span<uint8_t> data, const frc::Rotation2d& value) {
wpi::PackStruct<kValueOff>(data, value.Radians().value());
}

View File

@@ -10,18 +10,12 @@ constexpr size_t kQOff = 0;
using StructType = wpi::Struct<frc::Rotation3d>;
frc::Rotation3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::Rotation3d StructType::Unpack(std::span<const uint8_t> data) {
return frc::Rotation3d{
wpi::UnpackStruct<frc::Quaternion, kQOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Rotation3d& value) {
void StructType::Pack(std::span<uint8_t> data, const frc::Rotation3d& value) {
wpi::PackStruct<kQOff>(data, value.GetQuaternion());
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Quaternion>(fn);
}

View File

@@ -7,26 +7,19 @@
namespace {
constexpr size_t kTranslationOff = 0;
constexpr size_t kRotationOff =
kTranslationOff + wpi::Struct<frc::Translation2d>::kSize;
kTranslationOff + wpi::GetStructSize<frc::Translation2d>();
} // namespace
using StructType = wpi::Struct<frc::Transform2d>;
frc::Transform2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::Transform2d StructType::Unpack(std::span<const uint8_t> data) {
return frc::Transform2d{
wpi::UnpackStruct<frc::Translation2d, kTranslationOff>(data),
wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Transform2d& value) {
void StructType::Pack(std::span<uint8_t> data, const frc::Transform2d& value) {
wpi::PackStruct<kTranslationOff>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}

View File

@@ -7,26 +7,19 @@
namespace {
constexpr size_t kTranslationOff = 0;
constexpr size_t kRotationOff =
kTranslationOff + wpi::Struct<frc::Translation3d>::kSize;
kTranslationOff + wpi::GetStructSize<frc::Translation3d>();
} // namespace
using StructType = wpi::Struct<frc::Transform3d>;
frc::Transform3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::Transform3d StructType::Unpack(std::span<const uint8_t> data) {
return frc::Transform3d{
wpi::UnpackStruct<frc::Translation3d, kTranslationOff>(data),
wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Transform3d& value) {
void StructType::Pack(std::span<uint8_t> data, const frc::Transform3d& value) {
wpi::PackStruct<kTranslationOff>(data, value.Translation());
wpi::PackStruct<kRotationOff>(data, value.Rotation());
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation3d>(fn);
wpi::ForEachStructSchema<frc::Rotation3d>(fn);
}

View File

@@ -11,14 +11,14 @@ constexpr size_t kYOff = kXOff + 8;
using StructType = wpi::Struct<frc::Translation2d>;
frc::Translation2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::Translation2d StructType::Unpack(std::span<const uint8_t> data) {
return frc::Translation2d{
units::meter_t{wpi::UnpackStruct<double, kXOff>(data)},
units::meter_t{wpi::UnpackStruct<double, kYOff>(data)},
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
void StructType::Pack(std::span<uint8_t> data,
const frc::Translation2d& value) {
wpi::PackStruct<kXOff>(data, value.X().value());
wpi::PackStruct<kYOff>(data, value.Y().value());

View File

@@ -12,7 +12,7 @@ constexpr size_t kZOff = kYOff + 8;
using StructType = wpi::Struct<frc::Translation3d>;
frc::Translation3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::Translation3d StructType::Unpack(std::span<const uint8_t> data) {
return frc::Translation3d{
units::meter_t{wpi::UnpackStruct<double, kXOff>(data)},
units::meter_t{wpi::UnpackStruct<double, kYOff>(data)},
@@ -20,7 +20,7 @@ frc::Translation3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
void StructType::Pack(std::span<uint8_t> data,
const frc::Translation3d& value) {
wpi::PackStruct<kXOff>(data, value.X().value());
wpi::PackStruct<kYOff>(data, value.Y().value());

View File

@@ -12,7 +12,7 @@ constexpr size_t kDthetaOff = kDyOff + 8;
using StructType = wpi::Struct<frc::Twist2d>;
frc::Twist2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::Twist2d StructType::Unpack(std::span<const uint8_t> data) {
return frc::Twist2d{
units::meter_t{wpi::UnpackStruct<double, kDxOff>(data)},
units::meter_t{wpi::UnpackStruct<double, kDyOff>(data)},
@@ -20,8 +20,7 @@ frc::Twist2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Twist2d& value) {
void StructType::Pack(std::span<uint8_t> data, const frc::Twist2d& value) {
wpi::PackStruct<kDxOff>(data, value.dx.value());
wpi::PackStruct<kDyOff>(data, value.dy.value());
wpi::PackStruct<kDthetaOff>(data, value.dtheta.value());

View File

@@ -15,7 +15,7 @@ constexpr size_t kRzOff = kRyOff + 8;
using StructType = wpi::Struct<frc::Twist3d>;
frc::Twist3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::Twist3d StructType::Unpack(std::span<const uint8_t> data) {
return frc::Twist3d{
units::meter_t{wpi::UnpackStruct<double, kDxOff>(data)},
units::meter_t{wpi::UnpackStruct<double, kDyOff>(data)},
@@ -26,8 +26,7 @@ frc::Twist3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::Twist3d& value) {
void StructType::Pack(std::span<uint8_t> data, const frc::Twist3d& value) {
wpi::PackStruct<kDxOff>(data, value.dx.value());
wpi::PackStruct<kDyOff>(data, value.dy.value());
wpi::PackStruct<kDzOff>(data, value.dz.value());

View File

@@ -12,7 +12,7 @@ constexpr size_t kOmegaOff = kVyOff + 8;
using StructType = wpi::Struct<frc::ChassisSpeeds>;
frc::ChassisSpeeds StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::ChassisSpeeds StructType::Unpack(std::span<const uint8_t> data) {
return frc::ChassisSpeeds{
units::meters_per_second_t{wpi::UnpackStruct<double, kVxOff>(data)},
units::meters_per_second_t{wpi::UnpackStruct<double, kVyOff>(data)},
@@ -20,7 +20,7 @@ frc::ChassisSpeeds StructType::Unpack(std::span<const uint8_t, kSize> data) {
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
void StructType::Pack(std::span<uint8_t> data,
const frc::ChassisSpeeds& value) {
wpi::PackStruct<kVxOff>(data, value.vx.value());
wpi::PackStruct<kVyOff>(data, value.vy.value());

View File

@@ -11,13 +11,13 @@ constexpr size_t kTrackWidthOff = 0;
using StructType = wpi::Struct<frc::DifferentialDriveKinematics>;
frc::DifferentialDriveKinematics StructType::Unpack(
std::span<const uint8_t, kSize> data) {
std::span<const uint8_t> data) {
return frc::DifferentialDriveKinematics{
units::meter_t{wpi::UnpackStruct<double, kTrackWidthOff>(data)},
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
void StructType::Pack(std::span<uint8_t> data,
const frc::DifferentialDriveKinematics& value) {
wpi::PackStruct<kTrackWidthOff>(data, value.trackWidth.value());
}

View File

@@ -12,14 +12,14 @@ constexpr size_t kRightOff = kLeftOff + 8;
using StructType = wpi::Struct<frc::DifferentialDriveWheelSpeeds>;
frc::DifferentialDriveWheelSpeeds StructType::Unpack(
std::span<const uint8_t, kSize> data) {
std::span<const uint8_t> data) {
return frc::DifferentialDriveWheelSpeeds{
units::meters_per_second_t{wpi::UnpackStruct<double, kLeftOff>(data)},
units::meters_per_second_t{wpi::UnpackStruct<double, kRightOff>(data)},
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
void StructType::Pack(std::span<uint8_t> data,
const frc::DifferentialDriveWheelSpeeds& value) {
wpi::PackStruct<kLeftOff>(data, value.left.value());
wpi::PackStruct<kRightOff>(data, value.right.value());

View File

@@ -7,17 +7,16 @@
namespace {
constexpr size_t kFrontLeftOff = 0;
constexpr size_t kFrontRightOff =
kFrontLeftOff + wpi::Struct<frc::Translation2d>::kSize;
kFrontLeftOff + wpi::GetStructSize<frc::Translation2d>();
constexpr size_t kRearLeftOff =
kFrontRightOff + wpi::Struct<frc::Translation2d>::kSize;
kFrontRightOff + wpi::GetStructSize<frc::Translation2d>();
constexpr size_t kRearRightOff =
kRearLeftOff + wpi::Struct<frc::Translation2d>::kSize;
kRearLeftOff + wpi::GetStructSize<frc::Translation2d>();
} // namespace
using StructType = wpi::Struct<frc::MecanumDriveKinematics>;
frc::MecanumDriveKinematics StructType::Unpack(
std::span<const uint8_t, kSize> data) {
frc::MecanumDriveKinematics StructType::Unpack(std::span<const uint8_t> data) {
return frc::MecanumDriveKinematics{
wpi::UnpackStruct<frc::Translation2d, kFrontLeftOff>(data),
wpi::UnpackStruct<frc::Translation2d, kFrontRightOff>(data),
@@ -26,15 +25,10 @@ frc::MecanumDriveKinematics StructType::Unpack(
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
void StructType::Pack(std::span<uint8_t> data,
const frc::MecanumDriveKinematics& value) {
wpi::PackStruct<kFrontLeftOff>(data, value.GetFrontLeftWheel());
wpi::PackStruct<kFrontRightOff>(data, value.GetFrontRightWheel());
wpi::PackStruct<kRearLeftOff>(data, value.GetRearLeftWheel());
wpi::PackStruct<kRearRightOff>(data, value.GetRearRightWheel());
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
}

View File

@@ -14,7 +14,7 @@ constexpr size_t kRearRightOff = kRearLeftOff + 8;
using StructType = wpi::Struct<frc::MecanumDriveWheelPositions>;
frc::MecanumDriveWheelPositions StructType::Unpack(
std::span<const uint8_t, kSize> data) {
std::span<const uint8_t> data) {
return frc::MecanumDriveWheelPositions{
units::meter_t{wpi::UnpackStruct<double, kFrontLeftOff>(data)},
units::meter_t{wpi::UnpackStruct<double, kFrontRightOff>(data)},
@@ -23,7 +23,7 @@ frc::MecanumDriveWheelPositions StructType::Unpack(
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
void StructType::Pack(std::span<uint8_t> data,
const frc::MecanumDriveWheelPositions& value) {
wpi::PackStruct<kFrontLeftOff>(data, value.frontLeft.value());
wpi::PackStruct<kFrontRightOff>(data, value.frontRight.value());

View File

@@ -13,8 +13,7 @@ constexpr size_t kRearRightOff = kRearLeftOff + 8;
using StructType = wpi::Struct<frc::MecanumDriveWheelSpeeds>;
frc::MecanumDriveWheelSpeeds StructType::Unpack(
std::span<const uint8_t, kSize> data) {
frc::MecanumDriveWheelSpeeds StructType::Unpack(std::span<const uint8_t> data) {
return frc::MecanumDriveWheelSpeeds{
units::meters_per_second_t{
wpi::UnpackStruct<double, kFrontLeftOff>(data)},
@@ -26,7 +25,7 @@ frc::MecanumDriveWheelSpeeds StructType::Unpack(
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
void StructType::Pack(std::span<uint8_t> data,
const frc::MecanumDriveWheelSpeeds& value) {
wpi::PackStruct<kFrontLeftOff>(data, value.frontLeft.value());
wpi::PackStruct<kFrontRightOff>(data, value.frontRight.value());

View File

@@ -11,21 +11,15 @@ constexpr size_t kAngleOff = kDistanceOff + 8;
using StructType = wpi::Struct<frc::SwerveModulePosition>;
frc::SwerveModulePosition StructType::Unpack(
std::span<const uint8_t, kSize> data) {
frc::SwerveModulePosition StructType::Unpack(std::span<const uint8_t> data) {
return frc::SwerveModulePosition{
units::meter_t{wpi::UnpackStruct<double, kDistanceOff>(data)},
wpi::UnpackStruct<frc::Rotation2d, kAngleOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
void StructType::Pack(std::span<uint8_t> data,
const frc::SwerveModulePosition& value) {
wpi::PackStruct<kDistanceOff>(data, value.distance.value());
wpi::PackStruct<kAngleOff>(data, value.angle);
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}

View File

@@ -11,21 +11,15 @@ constexpr size_t kAngleOff = kSpeedOff + 8;
using StructType = wpi::Struct<frc::SwerveModuleState>;
frc::SwerveModuleState StructType::Unpack(
std::span<const uint8_t, kSize> data) {
frc::SwerveModuleState StructType::Unpack(std::span<const uint8_t> data) {
return frc::SwerveModuleState{
units::meters_per_second_t{wpi::UnpackStruct<double, kSpeedOff>(data)},
wpi::UnpackStruct<frc::Rotation2d, kAngleOff>(data),
};
}
void StructType::Pack(std::span<uint8_t, kSize> data,
void StructType::Pack(std::span<uint8_t> data,
const frc::SwerveModuleState& value) {
wpi::PackStruct<kSpeedOff>(data, value.speed.value());
wpi::PackStruct<kAngleOff>(data, value.angle);
}
void StructType::ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}

View File

@@ -11,16 +11,22 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose2d> {
static constexpr std::string_view kTypeString = "struct:Pose2d";
static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
wpi::Struct<frc::Rotation2d>::kSize;
static constexpr std::string_view kSchema =
"Translation2d translation;Rotation2d rotation";
static constexpr std::string_view GetTypeString() { return "struct:Pose2d"; }
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Translation2d>() +
wpi::GetStructSize<frc::Rotation2d>();
}
static constexpr std::string_view GetSchema() {
return "Translation2d translation;Rotation2d rotation";
}
static frc::Pose2d Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data, const frc::Pose2d& value);
static frc::Pose2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Pose2d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}
};
static_assert(wpi::HasNestedStruct<frc::Pose2d>);

View File

@@ -11,16 +11,22 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose3d> {
static constexpr std::string_view kTypeString = "struct:Pose3d";
static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
wpi::Struct<frc::Rotation3d>::kSize;
static constexpr std::string_view kSchema =
"Translation3d translation;Rotation3d rotation";
static constexpr std::string_view GetTypeString() { return "struct:Pose3d"; }
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Translation3d>() +
wpi::GetStructSize<frc::Rotation3d>();
}
static constexpr std::string_view GetSchema() {
return "Translation3d translation;Rotation3d rotation";
}
static frc::Pose3d Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data, const frc::Pose3d& value);
static frc::Pose3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Pose3d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation3d>(fn);
wpi::ForEachStructSchema<frc::Rotation3d>(fn);
}
};
static_assert(wpi::HasNestedStruct<frc::Pose3d>);

View File

@@ -11,12 +11,14 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Quaternion> {
static constexpr std::string_view kTypeString = "struct:Quaternion";
static constexpr size_t kSize = 32;
static constexpr std::string_view kSchema =
"double w;double x;double y;double z";
static 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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Quaternion& value);
static frc::Quaternion Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Quaternion& value);
};

View File

@@ -11,11 +11,12 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation2d> {
static constexpr std::string_view kTypeString = "struct:Rotation2d";
static constexpr size_t kSize = 8;
static constexpr std::string_view kSchema = "double value";
static 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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Rotation2d& value);
static frc::Rotation2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Rotation2d& value);
};

View File

@@ -11,15 +11,20 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation3d> {
static constexpr std::string_view kTypeString = "struct:Rotation3d";
static constexpr size_t kSize = wpi::Struct<frc::Quaternion>::kSize;
static constexpr std::string_view kSchema = "Quaternion q";
static constexpr std::string_view GetTypeString() {
return "struct:Rotation3d";
}
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Quaternion>();
}
static constexpr std::string_view GetSchema() { return "Quaternion q"; }
static frc::Rotation3d Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Rotation3d& value);
static frc::Rotation3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Rotation3d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Quaternion>(fn);
}
};
static_assert(wpi::HasNestedStruct<frc::Rotation3d>);

View File

@@ -11,17 +11,24 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform2d> {
static constexpr std::string_view kTypeString = "struct:Transform2d";
static constexpr size_t kSize = wpi::Struct<frc::Translation2d>::kSize +
wpi::Struct<frc::Rotation2d>::kSize;
static constexpr std::string_view kSchema =
"Translation2d translation;Rotation2d rotation";
static constexpr std::string_view GetTypeString() {
return "struct:Transform2d";
}
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Translation2d>() +
wpi::GetStructSize<frc::Rotation2d>();
}
static constexpr std::string_view GetSchema() {
return "Translation2d translation;Rotation2d rotation";
}
static frc::Transform2d Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Transform2d& value);
static frc::Transform2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Transform2d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}
};
static_assert(wpi::HasNestedStruct<frc::Transform2d>);

View File

@@ -11,17 +11,24 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform3d> {
static constexpr std::string_view kTypeString = "struct:Transform3d";
static constexpr size_t kSize = wpi::Struct<frc::Translation3d>::kSize +
wpi::Struct<frc::Rotation3d>::kSize;
static constexpr std::string_view kSchema =
"Translation3d translation;Rotation3d rotation";
static constexpr std::string_view GetTypeString() {
return "struct:Transform3d";
}
static constexpr size_t GetSize() {
return wpi::GetStructSize<frc::Translation3d>() +
wpi::GetStructSize<frc::Rotation3d>();
}
static constexpr std::string_view GetSchema() {
return "Translation3d translation;Rotation3d rotation";
}
static frc::Transform3d Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Transform3d& value);
static frc::Transform3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Transform3d& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation3d>(fn);
wpi::ForEachStructSchema<frc::Rotation3d>(fn);
}
};
static_assert(wpi::HasNestedStruct<frc::Transform3d>);

View File

@@ -11,11 +11,12 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation2d> {
static constexpr std::string_view kTypeString = "struct:Translation2d";
static constexpr size_t kSize = 16;
static constexpr std::string_view kSchema = "double x;double y";
static 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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Translation2d& value);
static frc::Translation2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Translation2d& value);
};

View File

@@ -11,11 +11,14 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation3d> {
static constexpr std::string_view kTypeString = "struct:Translation3d";
static constexpr size_t kSize = 24;
static constexpr std::string_view kSchema = "double x;double y;double z";
static 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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::Translation3d& value);
static frc::Translation3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Translation3d& value);
};

View File

@@ -11,11 +11,12 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist2d> {
static constexpr std::string_view kTypeString = "struct:Twist2d";
static constexpr size_t kSize = 24;
static constexpr std::string_view kSchema =
"double dx;double dy;double dtheta";
static 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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data, const frc::Twist2d& value);
static frc::Twist2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Twist2d& value);
};

View File

@@ -11,11 +11,12 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist3d> {
static constexpr std::string_view kTypeString = "struct:Twist3d";
static constexpr size_t kSize = 48;
static constexpr std::string_view kSchema =
"double dx;double dy;double dz;double rx;double ry;double rz";
static 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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data, const frc::Twist3d& value);
static frc::Twist3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Twist3d& value);
};

View File

@@ -11,12 +11,14 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::ChassisSpeeds> {
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::ChassisSpeeds& value);
static frc::ChassisSpeeds Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::ChassisSpeeds& value);
};

View File

@@ -11,13 +11,13 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveKinematics> {
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
static frc::DifferentialDriveKinematics Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::DifferentialDriveKinematics& value);
};

View File

@@ -11,13 +11,16 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveWheelSpeeds> {
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::DifferentialDriveWheelSpeeds& value);
};

View File

@@ -11,19 +11,24 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::MecanumDriveKinematics> {
static constexpr std::string_view kTypeString =
"struct:MecanumDriveKinematics";
static constexpr size_t kSize = 4 * wpi::Struct<frc::Translation2d>::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<frc::Translation2d>();
}
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
static frc::MecanumDriveKinematics Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::MecanumDriveKinematics& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Translation2d>(fn);
}
};
static_assert(wpi::HasNestedStruct<frc::MecanumDriveKinematics>);

View File

@@ -11,14 +11,16 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::MecanumDriveWheelPositions> {
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
static frc::MecanumDriveWheelPositions Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::MecanumDriveWheelPositions& value);
};

View File

@@ -11,14 +11,16 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::MecanumDriveWheelSpeeds> {
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<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
static frc::MecanumDriveWheelSpeeds Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::MecanumDriveWheelSpeeds& value);
};

View File

@@ -11,16 +11,23 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::SwerveModulePosition> {
static constexpr std::string_view kTypeString = "struct:SwerveModulePosition";
static constexpr size_t kSize = 8 + wpi::Struct<frc::Rotation2d>::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<frc::Rotation2d>();
}
static constexpr std::string_view GetSchema() {
return "double distance;Rotation2d angle";
}
static frc::SwerveModulePosition Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
static frc::SwerveModulePosition Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::SwerveModulePosition& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}
};
static_assert(wpi::HasNestedStruct<frc::SwerveModulePosition>);

View File

@@ -11,15 +11,23 @@
template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::SwerveModuleState> {
static constexpr std::string_view kTypeString = "struct:SwerveModuleState";
static constexpr size_t kSize = 8 + wpi::Struct<frc::Rotation2d>::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<frc::Rotation2d>();
}
static constexpr std::string_view GetSchema() {
return "double speed;Rotation2d angle";
}
static frc::SwerveModuleState Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
static frc::SwerveModuleState Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::SwerveModuleState& value);
static void ForEachNested(
std::invocable<std::string_view, std::string_view> auto fn);
std::invocable<std::string_view, std::string_view> auto fn) {
wpi::ForEachStructSchema<frc::Rotation2d>(fn);
}
};
static_assert(wpi::HasNestedStruct<frc::SwerveModuleState>);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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);

View File

@@ -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<uint8_t, 128> buf;
buf.resize_for_overwrite(S::GetSize());
S::Pack(buf, data);
m_log->AppendRaw(m_entry, buf, timestamp);
}
}
};

View File

@@ -7,6 +7,7 @@
#include <stdint.h>
#include <concepts>
#include <memory>
#include <span>
#include <string>
#include <string_view>
@@ -14,12 +15,14 @@
#include <utility>
#include <vector>
#include <fmt/format.h>
#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<const uint8_t, kSize>): function for deserialization
* - void Pack(std::span<uint8_t, kSize>, 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<T>.
*/
template <typename T>
concept StructSerializable =
requires(std::span<const uint8_t> in, std::span<uint8_t> out, T&& value) {
typename Struct<typename std::remove_cvref_t<T>>;
{
Struct<typename std::remove_cvref_t<T>>::kTypeString
} -> std::convertible_to<std::string_view>;
{
Struct<typename std::remove_cvref_t<T>>::kSize
} -> std::convertible_to<size_t>;
{
Struct<typename std::remove_cvref_t<T>>::kSchema
} -> std::convertible_to<std::string_view>;
{
Struct<typename std::remove_cvref_t<T>>::Unpack(
in.subspan<0, Struct<typename std::remove_cvref_t<T>>::kSize>())
} -> std::same_as<typename std::remove_cvref_t<T>>;
Struct<typename std::remove_cvref_t<T>>::Pack(
out.subspan<0, Struct<typename std::remove_cvref_t<T>>::kSize>(),
std::forward<T>(value));
};
concept StructSerializable = requires(std::span<const uint8_t> in,
std::span<uint8_t> out, T&& value) {
typename Struct<typename std::remove_cvref_t<T>>;
{
Struct<typename std::remove_cvref_t<T>>::GetTypeString()
} -> std::convertible_to<std::string_view>;
{
Struct<typename std::remove_cvref_t<T>>::GetSize()
} -> std::convertible_to<size_t>;
{
Struct<typename std::remove_cvref_t<T>>::GetSchema()
} -> std::convertible_to<std::string_view>;
{
Struct<typename std::remove_cvref_t<T>>::Unpack(in)
} -> std::same_as<typename std::remove_cvref_t<T>>;
Struct<typename std::remove_cvref_t<T>>::Pack(out, std::forward<T>(value));
};
/**
* Specifies that a type is capable of in-place raw struct deserialization.
@@ -87,8 +92,7 @@ concept StructSerializable =
template <typename T>
concept MutableStructSerializable =
StructSerializable<T> && requires(T* out, std::span<const uint8_t> in) {
Struct<typename std::remove_cvref_t<T>>::UnpackInto(
out, in.subspan<0, Struct<typename std::remove_cvref_t<T>>::kSize>());
Struct<typename std::remove_cvref_t<T>>::UnpackInto(out, in);
};
/**
@@ -115,7 +119,7 @@ concept HasNestedStruct =
* @return Deserialized object
*/
template <StructSerializable T>
inline T UnpackStruct(std::span<const uint8_t, Struct<T>::kSize> data) {
inline T UnpackStruct(std::span<const uint8_t> data) {
return Struct<T>::Unpack(data);
}
@@ -130,7 +134,7 @@ inline T UnpackStruct(std::span<const uint8_t, Struct<T>::kSize> data) {
*/
template <StructSerializable T, size_t Offset>
inline T UnpackStruct(std::span<const uint8_t> data) {
return Struct<T>::Unpack(data.template subspan<Offset, Struct<T>::kSize>());
return Struct<T>::Unpack(data.subspan(Offset));
}
/**
@@ -140,9 +144,7 @@ inline T UnpackStruct(std::span<const uint8_t> data) {
* @param value object
*/
template <StructSerializable T>
inline void PackStruct(
std::span<uint8_t, Struct<typename std::remove_cvref_t<T>>::kSize> data,
T&& value) {
inline void PackStruct(std::span<uint8_t> data, T&& value) {
Struct<typename std::remove_cvref_t<T>>::Pack(data, std::forward<T>(value));
}
@@ -156,10 +158,8 @@ inline void PackStruct(
*/
template <size_t Offset, StructSerializable T>
inline void PackStruct(std::span<uint8_t> data, T&& value) {
Struct<typename std::remove_cvref_t<T>>::Pack(
data.template subspan<Offset,
Struct<typename std::remove_cvref_t<T>>::kSize>(),
std::forward<T>(value));
Struct<typename std::remove_cvref_t<T>>::Pack(data.subspan(Offset),
std::forward<T>(value));
}
/**
@@ -169,8 +169,7 @@ inline void PackStruct(std::span<uint8_t> data, T&& value) {
* @param data raw struct data
*/
template <StructSerializable T>
inline void UnpackStructInto(T* out,
std::span<const uint8_t, Struct<T>::kSize> data) {
inline void UnpackStructInto(T* out, std::span<const uint8_t> data) {
if constexpr (MutableStructSerializable<T>) {
Struct<T>::UnpackInto(out, data);
} else {
@@ -190,8 +189,7 @@ inline void UnpackStructInto(T* out,
template <size_t Offset, StructSerializable T>
inline void UnpackStructInto(T* out, std::span<const uint8_t> data) {
if constexpr (MutableStructSerializable<T>) {
Struct<T>::UnpackInto(out,
data.template subspan<Offset, Struct<T>::kSize>());
Struct<T>::UnpackInto(out, data.subspan(Offset));
} else {
*out = UnpackStruct<T, Offset>(data);
}
@@ -205,59 +203,86 @@ inline void UnpackStructInto(T* out, std::span<const uint8_t> data) {
*/
template <StructSerializable T>
constexpr auto GetStructTypeString() {
return Struct<T>::kTypeString;
return Struct<T>::GetTypeString();
}
/**
* Get the size for a raw struct serializable type
*
* @tparam T serializable type
* @return size
*/
template <StructSerializable T>
constexpr size_t GetStructSize() {
return Struct<T>::GetSize();
}
template <StructSerializable T, size_t N>
consteval auto MakeStructArrayTypeString() {
using namespace literals;
if constexpr (N == std::dynamic_extent) {
return Concat(
ct_string<char, std::char_traits<char>, Struct<T>::kTypeString.size()>{
Struct<T>::kTypeString},
"[]"_ct_string);
constexpr auto MakeStructArrayTypeString() {
if constexpr (is_constexpr([] { Struct<T>::GetTypeString(); })) {
constexpr auto typeString = Struct<T>::GetTypeString();
using namespace literals;
if constexpr (N == std::dynamic_extent) {
return Concat(
ct_string<char, std::char_traits<char>, typeString.size()>{
typeString},
"[]"_ct_string);
} else {
return Concat(
ct_string<char, std::char_traits<char>, typeString.size()>{
typeString},
"["_ct_string, NumToCtString<N>(), "]"_ct_string);
}
} else {
return Concat(
ct_string<char, std::char_traits<char>, Struct<T>::kTypeString.size()>{
Struct<T>::kTypeString},
"["_ct_string, NumToCtString<N>(), "]"_ct_string);
if constexpr (N == std::dynamic_extent) {
return fmt::format("{}[]", Struct<T>::GetTypeString());
} else {
return fmt::format("{}[{}]", Struct<T>::GetTypeString(), N);
}
}
}
template <StructSerializable T, size_t N>
consteval auto MakeStructArraySchema() {
using namespace literals;
if constexpr (N == std::dynamic_extent) {
return Concat(
ct_string<char, std::char_traits<char>, Struct<T>::kSchema.size()>{
Struct<T>::kSchema},
"[]"_ct_string);
if constexpr (is_constexpr([] { Struct<T>::GetSchema(); })) {
constexpr auto schema = Struct<T>::GetSchema();
using namespace literals;
if constexpr (N == std::dynamic_extent) {
return Concat(
ct_string<char, std::char_traits<char>, schema.size()>{schema},
"[]"_ct_string);
} else {
return Concat(
ct_string<char, std::char_traits<char>, schema.size()>{schema},
"["_ct_string, NumToCtString<N>(), "]"_ct_string);
}
} else {
return Concat(
ct_string<char, std::char_traits<char>, Struct<T>::kSchema.size()>{
Struct<T>::kSchema},
"["_ct_string, NumToCtString<N>(), "]"_ct_string);
if constexpr (N == std::dynamic_extent) {
return fmt::format("{}[]", Struct<T>::GetSchema());
} else {
return fmt::format("{}[{}]", Struct<T>::GetSchema(), N);
}
}
}
template <StructSerializable T>
constexpr std::string_view GetStructSchema() {
return Struct<T>::kSchema;
return Struct<T>::GetSchema();
}
template <StructSerializable T>
constexpr std::span<const uint8_t> GetStructSchemaBytes() {
return {reinterpret_cast<const uint8_t*>(Struct<T>::kSchema.data()),
Struct<T>::kSchema.size()};
auto schema = Struct<T>::GetSchema();
return {reinterpret_cast<const uint8_t*>(schema.data()), schema.size()};
}
template <StructSerializable T>
void ForEachStructSchema(
std::invocable<std::string_view, std::string_view> auto fn) {
if constexpr (HasNestedStruct<T>) {
Struct<T>::ForEachNested(fn);
Struct<typename std::remove_cvref_t<T>>::ForEachNested(fn);
}
fn(Struct<T>::kTypeString, Struct<T>::kSchema);
fn(Struct<T>::GetTypeString(), Struct<T>::GetSchema());
}
template <StructSerializable T>
@@ -282,24 +307,25 @@ class StructArrayBuffer {
#endif
std::invocable<F, std::span<const uint8_t>>
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<uint8_t, S::kSize>{out, out + S::kSize},
S::Pack(std::span<uint8_t>{std::to_address(out), size},
std::forward<decltype(val)>(val));
out += S::kSize;
out += size;
}
func(std::span<uint8_t>{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<uint8_t, S::kSize>{out, out + S::kSize},
S::Pack(std::span<uint8_t>{std::to_address(out), size},
std::forward<decltype(val)>(val));
out += S::kSize;
out += size;
}
func(m_buf);
}
@@ -315,36 +341,38 @@ class StructArrayBuffer {
*/
template <StructSerializable T, size_t N>
struct Struct<std::array<T, N>> {
static constexpr auto kTypeString = MakeStructArrayTypeString<T, N>();
static constexpr size_t kSize = N * Struct<T>::kSize;
static constexpr auto kSchema = MakeStructArraySchema<T, N>();
static std::array<T, N> Unpack(std::span<const uint8_t, kSize> data) {
static constexpr auto GetTypeString() {
return MakeStructArrayTypeString<T, N>();
}
static constexpr size_t GetSize() { return N * GetStructSize<T>(); }
static constexpr auto GetSchema() { return MakeStructArraySchema<T, N>(); }
static std::array<T, N> Unpack(std::span<const uint8_t> data) {
auto size = GetStructSize<T>();
std::array<T, N> result;
for (size_t i = 0; i < N; ++i) {
result[i] = UnpackStruct<T, 0>(data);
data = data.subspan(Struct<T>::kSize);
data = data.subspan(size);
}
return result;
}
static void Pack(std::span<uint8_t, kSize> data,
std::span<const T, N> values) {
static void Pack(std::span<uint8_t> data, std::span<const T, N> values) {
auto size = GetStructSize<T>();
std::span<uint8_t> unsizedData = data;
for (auto&& val : values) {
PackStruct<0>(unsizedData, val);
unsizedData = unsizedData.subspan(Struct<T>::kSize);
PackStruct(unsizedData, val);
unsizedData = unsizedData.subspan(size);
}
}
static void UnpackInto(std::array<T, N>* out,
std::span<const uint8_t, kSize> data) {
static void UnpackInto(std::array<T, N>* out, std::span<const uint8_t> data) {
UnpackInto(std::span{*out}, data);
}
// alternate span-based function
static void UnpackInto(std::span<T, N> out,
std::span<const uint8_t, kSize> data) {
static void UnpackInto(std::span<T, N> out, std::span<const uint8_t> data) {
auto size = GetStructSize<T>();
std::span<const uint8_t> unsizedData = data;
for (size_t i = 0; i < N; ++i) {
UnpackStructInto<0, T>(&out[i], unsizedData);
unsizedData = unsizedData.subspan(Struct<T>::kSize);
UnpackStructInto(&out[i], unsizedData);
unsizedData = unsizedData.subspan(size);
}
}
};
@@ -355,11 +383,11 @@ struct Struct<std::array<T, N>> {
*/
template <>
struct Struct<bool> {
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<const uint8_t, 1> data) { return data[0]; }
static void Pack(std::span<uint8_t, 1> 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<const uint8_t> data) { return data[0]; }
static void Pack(std::span<uint8_t> data, bool value) {
data[0] = static_cast<char>(value ? 1 : 0);
}
};
@@ -370,13 +398,11 @@ struct Struct<bool> {
*/
template <>
struct Struct<uint8_t> {
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<const uint8_t, 1> data) { return data[0]; }
static void Pack(std::span<uint8_t, 1> 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<const uint8_t> data) { return data[0]; }
static void Pack(std::span<uint8_t> data, uint8_t value) { data[0] = value; }
};
/**
@@ -400,13 +426,13 @@ struct Struct<int8_t> {
*/
template <>
struct Struct<uint16_t> {
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<const uint8_t, 2> 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<const uint8_t> data) {
return support::endian::read16le(data.data());
}
static void Pack(std::span<uint8_t, 2> data, uint16_t value) {
static void Pack(std::span<uint8_t> data, uint16_t value) {
support::endian::write16le(data.data(), value);
}
};
@@ -417,13 +443,13 @@ struct Struct<uint16_t> {
*/
template <>
struct Struct<int16_t> {
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<const uint8_t, 2> 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<const uint8_t> data) {
return support::endian::read16le(data.data());
}
static void Pack(std::span<uint8_t, 2> data, int16_t value) {
static void Pack(std::span<uint8_t> data, int16_t value) {
support::endian::write16le(data.data(), value);
}
};
@@ -434,13 +460,13 @@ struct Struct<int16_t> {
*/
template <>
struct Struct<uint32_t> {
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<const uint8_t, 4> 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<const uint8_t> data) {
return support::endian::read32le(data.data());
}
static void Pack(std::span<uint8_t, 4> data, uint32_t value) {
static void Pack(std::span<uint8_t> data, uint32_t value) {
support::endian::write32le(data.data(), value);
}
};
@@ -451,13 +477,13 @@ struct Struct<uint32_t> {
*/
template <>
struct Struct<int32_t> {
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<const uint8_t, 4> 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<const uint8_t> data) {
return support::endian::read32le(data.data());
}
static void Pack(std::span<uint8_t, 4> data, int32_t value) {
static void Pack(std::span<uint8_t> data, int32_t value) {
support::endian::write32le(data.data(), value);
}
};
@@ -468,13 +494,13 @@ struct Struct<int32_t> {
*/
template <>
struct Struct<uint64_t> {
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<const uint8_t, 8> 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<const uint8_t> data) {
return support::endian::read64le(data.data());
}
static void Pack(std::span<uint8_t, 8> data, uint64_t value) {
static void Pack(std::span<uint8_t> data, uint64_t value) {
support::endian::write64le(data.data(), value);
}
};
@@ -485,13 +511,13 @@ struct Struct<uint64_t> {
*/
template <>
struct Struct<int64_t> {
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<const uint8_t, 8> 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<const uint8_t> data) {
return support::endian::read64le(data.data());
}
static void Pack(std::span<uint8_t, 8> data, int64_t value) {
static void Pack(std::span<uint8_t> data, int64_t value) {
support::endian::write64le(data.data(), value);
}
};
@@ -502,13 +528,13 @@ struct Struct<int64_t> {
*/
template <>
struct Struct<float> {
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<const uint8_t, 4> 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<const uint8_t> data) {
return bit_cast<float>(support::endian::read32le(data.data()));
}
static void Pack(std::span<uint8_t, 4> data, float value) {
static void Pack(std::span<uint8_t> data, float value) {
support::endian::write32le(data.data(), bit_cast<uint32_t>(value));
}
};
@@ -519,13 +545,13 @@ struct Struct<float> {
*/
template <>
struct Struct<double> {
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<const uint8_t, 8> 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<const uint8_t> data) {
return bit_cast<double>(support::endian::read64le(data.data()));
}
static void Pack(std::span<uint8_t, 8> data, double value) {
static void Pack(std::span<uint8_t> data, double value) {
support::endian::write64le(data.data(), bit_cast<uint64_t>(value));
}
};