Skip to content

Commit

Permalink
[wpimath] Struct cleanup (#6011)
Browse files Browse the repository at this point in the history
  • Loading branch information
KangarooKoala authored Dec 5, 2023
1 parent 90757b9 commit 14c3ade
Show file tree
Hide file tree
Showing 32 changed files with 95 additions and 49 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ constexpr size_t kKaOff = kKvOff + 8;

using StructType = wpi::Struct<frc::ArmFeedforward>;

frc::ArmFeedforward StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::ArmFeedforward StructType::Unpack(std::span<const uint8_t> data) {
return frc::ArmFeedforward{
units::volt_t{wpi::UnpackStruct<double, kKsOff>(data)},
units::volt_t{wpi::UnpackStruct<double, kKgOff>(data)},
Expand All @@ -24,7 +24,7 @@ frc::ArmFeedforward 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::ArmFeedforward& value) {
wpi::PackStruct<kKsOff>(data, value.kS());
wpi::PackStruct<kKgOff>(data, value.kG());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,14 @@ constexpr size_t kRightOff = kLeftOff + 8;
using StructType = wpi::Struct<frc::DifferentialDriveWheelVoltages>;

frc::DifferentialDriveWheelVoltages StructType::Unpack(
std::span<const uint8_t, kSize> data) {
std::span<const uint8_t> data) {
return frc::DifferentialDriveWheelVoltages{
units::volt_t{wpi::UnpackStruct<double, kLeftOff>(data)},
units::volt_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::DifferentialDriveWheelVoltages& value) {
wpi::PackStruct<kLeftOff>(data, value.left.value());
wpi::PackStruct<kRightOff>(data, value.right.value());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,7 @@ constexpr size_t kKaOff = kKvOff + 8;

using StructType = wpi::Struct<frc::ElevatorFeedforward>;

frc::ElevatorFeedforward StructType::Unpack(
std::span<const uint8_t, kSize> data) {
frc::ElevatorFeedforward StructType::Unpack(std::span<const uint8_t> data) {
return frc::ElevatorFeedforward{
units::volt_t{wpi::UnpackStruct<double, kKsOff>(data)},
units::volt_t{wpi::UnpackStruct<double, kKgOff>(data)},
Expand All @@ -25,7 +24,7 @@ frc::ElevatorFeedforward StructType::Unpack(
};
}

void StructType::Pack(std::span<uint8_t, kSize> data,
void StructType::Pack(std::span<uint8_t> data,
const frc::ElevatorFeedforward& value) {
wpi::PackStruct<kKsOff>(data, value.kS());
wpi::PackStruct<kKgOff>(data, value.kG());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ constexpr size_t kFreeSpeedOff = kFreeCurrentOff + 8;

using StructType = wpi::Struct<frc::DCMotor>;

frc::DCMotor StructType::Unpack(std::span<const uint8_t, kSize> data) {
frc::DCMotor StructType::Unpack(std::span<const uint8_t> data) {
return frc::DCMotor{
units::volt_t{wpi::UnpackStruct<double, kNominalVoltageOff>(data)},
units::newton_meter_t{wpi::UnpackStruct<double, kStallTorqueOff>(data)},
Expand All @@ -25,8 +25,7 @@ frc::DCMotor StructType::Unpack(std::span<const uint8_t, kSize> data) {
};
}

void StructType::Pack(std::span<uint8_t, kSize> data,
const frc::DCMotor& value) {
void StructType::Pack(std::span<uint8_t> data, const frc::DCMotor& value) {
wpi::PackStruct<kNominalVoltageOff>(data, value.nominalVoltage.value());
wpi::PackStruct<kStallTorqueOff>(data, value.stallTorque.value());
wpi::PackStruct<kStallCurrentOff>(data, value.stallCurrent.value());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,12 +11,16 @@

template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::ArmFeedforward> {
static constexpr std::string_view kTypeString = "struct:ArmFeedforward";
static constexpr size_t kSize = 32;
static constexpr std::string_view kSchema =
"double ks;double kg;double kv;double ka";
static constexpr std::string_view GetTypeString() {
return "struct:ArmFeedforward";
}
static constexpr size_t GetSize() { return 32; }
static constexpr std::string_view GetSchema() {
return "double ks;double kg;double kv;double ka";
}

static frc::ArmFeedforward Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
const frc::ArmFeedforward& value);
static frc::ArmFeedforward Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::ArmFeedforward& value);
};

static_assert(wpi::StructSerializable<frc::ArmFeedforward>);
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,18 @@

template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveWheelVoltages> {
static constexpr std::string_view kTypeString =
"struct:DifferentialDriveWheelVoltages";
static constexpr size_t kSize = 16;
static constexpr std::string_view kSchema = "double left;double right";
static constexpr std::string_view GetTypeString() {
return "struct:DifferentialDriveWheelVoltages";
}
static constexpr size_t GetSize() { return 16; }
static constexpr std::string_view GetSchema() {
return "double left;double right";
}

static frc::DifferentialDriveWheelVoltages 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::DifferentialDriveWheelVoltages& value);
};

static_assert(wpi::StructSerializable<frc::DifferentialDriveWheelVoltages>);
Original file line number Diff line number Diff line change
Expand Up @@ -11,12 +11,17 @@

template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::ElevatorFeedforward> {
static constexpr std::string_view kTypeString = "struct:ElevatorFeedforward";
static constexpr size_t kSize = 32;
static constexpr std::string_view kSchema =
"double ks;double kg;double kv;double ka";
static constexpr std::string_view GetTypeString() {
return "struct:ElevatorFeedforward";
}
static constexpr size_t GetSize() { return 32; }
static constexpr std::string_view GetSchema() {
return "double ks;double kg;double kv;double ka";
}

static frc::ElevatorFeedforward Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data,
static frc::ElevatorFeedforward Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data,
const frc::ElevatorFeedforward& value);
};

static_assert(wpi::StructSerializable<frc::ElevatorFeedforward>);
Original file line number Diff line number Diff line change
Expand Up @@ -29,4 +29,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose2d> {
}
};

static_assert(wpi::StructSerializable<frc::Pose2d>);
static_assert(wpi::HasNestedStruct<frc::Pose2d>);
Original file line number Diff line number Diff line change
Expand Up @@ -29,4 +29,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::Pose3d> {
}
};

static_assert(wpi::StructSerializable<frc::Pose3d>);
static_assert(wpi::HasNestedStruct<frc::Pose3d>);
Original file line number Diff line number Diff line change
Expand Up @@ -22,3 +22,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::Quaternion> {
static frc::Quaternion Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Quaternion& value);
};

static_assert(wpi::StructSerializable<frc::Quaternion>);
Original file line number Diff line number Diff line change
Expand Up @@ -20,3 +20,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation2d> {
static frc::Rotation2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Rotation2d& value);
};

static_assert(wpi::StructSerializable<frc::Rotation2d>);
Original file line number Diff line number Diff line change
Expand Up @@ -27,4 +27,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::Rotation3d> {
}
};

static_assert(wpi::StructSerializable<frc::Rotation3d>);
static_assert(wpi::HasNestedStruct<frc::Rotation3d>);
Original file line number Diff line number Diff line change
Expand Up @@ -31,4 +31,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform2d> {
}
};

static_assert(wpi::StructSerializable<frc::Transform2d>);
static_assert(wpi::HasNestedStruct<frc::Transform2d>);
Original file line number Diff line number Diff line change
Expand Up @@ -31,4 +31,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::Transform3d> {
}
};

static_assert(wpi::StructSerializable<frc::Transform3d>);
static_assert(wpi::HasNestedStruct<frc::Transform3d>);
Original file line number Diff line number Diff line change
Expand Up @@ -20,3 +20,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation2d> {
static frc::Translation2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Translation2d& value);
};

static_assert(wpi::StructSerializable<frc::Translation2d>);
Original file line number Diff line number Diff line change
Expand Up @@ -22,3 +22,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::Translation3d> {
static frc::Translation3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Translation3d& value);
};

static_assert(wpi::StructSerializable<frc::Translation3d>);
Original file line number Diff line number Diff line change
Expand Up @@ -20,3 +20,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist2d> {
static frc::Twist2d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Twist2d& value);
};

static_assert(wpi::StructSerializable<frc::Twist2d>);
Original file line number Diff line number Diff line change
Expand Up @@ -20,3 +20,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::Twist3d> {
static frc::Twist3d Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::Twist3d& value);
};

static_assert(wpi::StructSerializable<frc::Twist3d>);
Original file line number Diff line number Diff line change
Expand Up @@ -22,3 +22,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::ChassisSpeeds> {
static frc::ChassisSpeeds Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::ChassisSpeeds& value);
};

static_assert(wpi::StructSerializable<frc::ChassisSpeeds>);
Original file line number Diff line number Diff line change
Expand Up @@ -21,3 +21,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveKinematics> {
static void Pack(std::span<uint8_t> data,
const frc::DifferentialDriveKinematics& value);
};

static_assert(wpi::StructSerializable<frc::DifferentialDriveKinematics>);
Original file line number Diff line number Diff line change
Expand Up @@ -24,3 +24,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::DifferentialDriveWheelSpeeds> {
static void Pack(std::span<uint8_t> data,
const frc::DifferentialDriveWheelSpeeds& value);
};

static_assert(wpi::StructSerializable<frc::DifferentialDriveWheelSpeeds>);
Original file line number Diff line number Diff line change
Expand Up @@ -31,4 +31,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::MecanumDriveKinematics> {
}
};

static_assert(wpi::StructSerializable<frc::MecanumDriveKinematics>);
static_assert(wpi::HasNestedStruct<frc::MecanumDriveKinematics>);
Original file line number Diff line number Diff line change
Expand Up @@ -24,3 +24,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::MecanumDriveWheelPositions> {
static void Pack(std::span<uint8_t> data,
const frc::MecanumDriveWheelPositions& value);
};

static_assert(wpi::StructSerializable<frc::MecanumDriveWheelPositions>);
Original file line number Diff line number Diff line change
Expand Up @@ -24,3 +24,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::MecanumDriveWheelSpeeds> {
static void Pack(std::span<uint8_t> data,
const frc::MecanumDriveWheelSpeeds& value);
};

static_assert(wpi::StructSerializable<frc::MecanumDriveWheelSpeeds>);
Original file line number Diff line number Diff line change
Expand Up @@ -30,4 +30,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::SwerveModulePosition> {
}
};

static_assert(wpi::StructSerializable<frc::SwerveModulePosition>);
static_assert(wpi::HasNestedStruct<frc::SwerveModulePosition>);
Original file line number Diff line number Diff line change
Expand Up @@ -30,4 +30,5 @@ struct WPILIB_DLLEXPORT wpi::Struct<frc::SwerveModuleState> {
}
};

static_assert(wpi::StructSerializable<frc::SwerveModuleState>);
static_assert(wpi::HasNestedStruct<frc::SwerveModuleState>);
Original file line number Diff line number Diff line change
Expand Up @@ -11,12 +11,16 @@

template <>
struct WPILIB_DLLEXPORT wpi::Struct<frc::DCMotor> {
static constexpr std::string_view kTypeString = "struct:DCMotor";
static constexpr size_t kSize = 40;
static constexpr std::string_view kSchema =
"double nominal_voltage;double stall_torque;double stall_current;double "
"free_current;double free_speed";
static constexpr std::string_view GetTypeString() { return "struct:DCMotor"; }
static constexpr size_t GetSize() { return 40; }
static constexpr std::string_view GetSchema() {
return "double nominal_voltage;double stall_torque;double "
"stall_current;double "
"free_current;double free_speed";
}

static frc::DCMotor Unpack(std::span<const uint8_t, kSize> data);
static void Pack(std::span<uint8_t, kSize> data, const frc::DCMotor& value);
static frc::DCMotor Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const frc::DCMotor& value);
};

static_assert(wpi::StructSerializable<frc::DCMotor>);
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,8 @@ const ArmFeedforward kExpectedData{Ks, Kg, Kv, Ka};
} // namespace

TEST(ArmFeedforwardStructTest, 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);

ArmFeedforward unpacked_data = StructType::Unpack(buffer);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@ const DifferentialDriveWheelVoltages kExpectedData{
} // namespace

TEST(DifferentialDriveWheelVoltagesStructTest, 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);

DifferentialDriveWheelVoltages unpacked_data = StructType::Unpack(buffer);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,8 @@ constexpr ElevatorFeedforward kExpectedData{Ks, Kg, Kv, Ka};
} // namespace

TEST(ElevatorFeedforwardStructTest, 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);

ElevatorFeedforward unpacked_data = StructType::Unpack(buffer);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,8 @@ const DCMotor kExpectedData = DCMotor{units::volt_t{1.91},
} // namespace

TEST(DCMotorStructTest, 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);

DCMotor unpacked_data = StructType::Unpack(buffer);
Expand Down
10 changes: 5 additions & 5 deletions wpiutil/src/main/native/include/wpi/struct/Struct.h
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,8 @@ struct Struct {};
* - 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
* - T Unpack(std::span<const uint8_t>): function for deserialization
* - void Pack(std::span<uint8_t>, T&& value): function for
* serialization
*
* If possible, the GetTypeString(), GetSize(), and GetSchema() functions should
Expand Down Expand Up @@ -411,9 +411,9 @@ struct Struct<uint8_t> {
*/
template <>
struct Struct<int8_t> {
static constexpr std::string_view kTypeString = "struct:int8";
static constexpr size_t kSize = 1;
static constexpr std::string_view kSchema = "int8 value";
static constexpr std::string_view GetTypeString() { return "struct:int8"; }
static constexpr size_t GetSize() { return 1; }
static constexpr std::string_view GetSchema() { return "int8 value"; }
static int8_t Unpack(std::span<const uint8_t, 1> data) { return data[0]; }
static void Pack(std::span<uint8_t, 1> data, int8_t value) {
data[0] = value;
Expand Down

0 comments on commit 14c3ade

Please sign in to comment.