From 573ac64bf473fea8e0aef69dd5192d8f8c6eecc6 Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Sat, 9 Mar 2024 12:09:02 -0600 Subject: [PATCH] [wpimath] Add structured data support for DifferentialDriveWheelPositions (#6412) --- .../edu/wpi/first/math/proto/Kinematics.java | 460 +++++++++++++++--- .../DifferentialDriveWheelPositions.java | 10 + .../DifferentialDriveWheelPositionsProto.java | 40 ++ ...DifferentialDriveWheelPositionsStruct.java | 45 ++ .../DifferentialDriveWheelPositionsProto.cpp | 34 ++ .../DifferentialDriveWheelPositionsStruct.cpp | 26 + .../DifferentialDriveWheelPositions.h | 3 + .../DifferentialDriveWheelPositionsProto.h | 19 + .../DifferentialDriveWheelPositionsStruct.h | 28 ++ wpimath/src/main/proto/kinematics.proto | 5 + ...erentialDriveWheelPositionsStructTest.java | 29 ++ ...ferentialDriveWheelPositionsStructTest.cpp | 27 + 12 files changed, 668 insertions(+), 58 deletions(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java create mode 100644 wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelPositionsProto.cpp create mode 100644 wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStruct.cpp create mode 100644 wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h create mode 100644 wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h create mode 100644 wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java create mode 100644 wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStructTest.cpp diff --git a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java index bd0c4450cb5..5494f0b6752 100644 --- a/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java +++ b/wpimath/src/generated/main/java/edu/wpi/first/math/proto/Kinematics.java @@ -19,61 +19,65 @@ import us.hebi.quickbuf.RepeatedMessage; public final class Kinematics { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3208, + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(3427, "ChBraW5lbWF0aWNzLnByb3RvEgl3cGkucHJvdG8aEGdlb21ldHJ5MmQucHJvdG8iTQoVUHJvdG9idWZD" + "aGFzc2lzU3BlZWRzEg4KAnZ4GAEgASgBUgJ2eBIOCgJ2eRgCIAEoAVICdnkSFAoFb21lZ2EYAyABKAFS" + "BW9tZWdhIkYKI1Byb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVLaW5lbWF0aWNzEh8KC3RyYWNrX3dpZHRo" + "GAEgASgBUgp0cmFja1dpZHRoIlAKJFByb3RvYnVmRGlmZmVyZW50aWFsRHJpdmVXaGVlbFNwZWVkcxIS" + - "CgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodCKkAgoeUHJvdG9idWZNZWNhbnVt" + - "RHJpdmVLaW5lbWF0aWNzEj8KCmZyb250X2xlZnQYASABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFu" + - "c2xhdGlvbjJkUglmcm9udExlZnQSQQoLZnJvbnRfcmlnaHQYAiABKAsyIC53cGkucHJvdG8uUHJvdG9i" + - "dWZUcmFuc2xhdGlvbjJkUgpmcm9udFJpZ2h0Ej0KCXJlYXJfbGVmdBgDIAEoCzIgLndwaS5wcm90by5Q" + - "cm90b2J1ZlRyYW5zbGF0aW9uMmRSCHJlYXJMZWZ0Ej8KCnJlYXJfcmlnaHQYBCABKAsyIC53cGkucHJv" + - "dG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUglyZWFyUmlnaHQinwEKIVByb3RvYnVmTWVjYW51bURyaXZl" + - "TW90b3JWb2x0YWdlcxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQY" + - "AiABKAFSCmZyb250UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0" + - "GAQgASgBUglyZWFyUmlnaHQioAEKIlByb3RvYnVmTWVjYW51bURyaXZlV2hlZWxQb3NpdGlvbnMSHQoK" + - "ZnJvbnRfbGVmdBgBIAEoAVIJZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0" + - "EhsKCXJlYXJfbGVmdBgDIAEoAVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0" + - "Ip0BCh9Qcm90b2J1Zk1lY2FudW1Ecml2ZVdoZWVsU3BlZWRzEh0KCmZyb250X2xlZnQYASABKAFSCWZy" + - "b250TGVmdBIfCgtmcm9udF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFS" + - "CHJlYXJMZWZ0Eh0KCnJlYXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCJbCh1Qcm90b2J1ZlN3ZXJ2ZURy" + - "aXZlS2luZW1hdGljcxI6Cgdtb2R1bGVzGAEgAygLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRp" + - "b24yZFIHbW9kdWxlcyJvChxQcm90b2J1ZlN3ZXJ2ZU1vZHVsZVBvc2l0aW9uEhoKCGRpc3RhbmNlGAEg", - "ASgBUghkaXN0YW5jZRIzCgVhbmdsZRgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJvdGF0aW9uMmRS" + - "BWFuZ2xlImYKGVByb3RvYnVmU3dlcnZlTW9kdWxlU3RhdGUSFAoFc3BlZWQYASABKAFSBXNwZWVkEjMK" + - "BWFuZ2xlGAIgASgLMh0ud3BpLnByb3RvLlByb3RvYnVmUm90YXRpb24yZFIFYW5nbGVCGgoYZWR1Lndw" + - "aS5maXJzdC5tYXRoLnByb3RvSocOCgYSBAAAPwEKCAoBDBIDAAASCggKAQISAwIAEgoJCgIDABIDBAAa" + - "CggKAQgSAwYAMQoJCgIIARIDBgAxCgoKAgQAEgQIAAwBCgoKAwQAARIDCAgdCgsKBAQAAgASAwkCEAoM" + - "CgUEAAIABRIDCQIICgwKBQQAAgABEgMJCQsKDAoFBAACAAMSAwkODwoLCgQEAAIBEgMKAhAKDAoFBAAC" + - "AQUSAwoCCAoMCgUEAAIBARIDCgkLCgwKBQQAAgEDEgMKDg8KCwoEBAACAhIDCwITCgwKBQQAAgIFEgML" + - "AggKDAoFBAACAgESAwsJDgoMCgUEAAICAxIDCxESCgoKAgQBEgQOABABCgoKAwQBARIDDggrCgsKBAQB" + - "AgASAw8CGQoMCgUEAQIABRIDDwIICgwKBQQBAgABEgMPCRQKDAoFBAECAAMSAw8XGAoKCgIEAhIEEgAV" + - "AQoKCgMEAgESAxIILAoLCgQEAgIAEgMTAhIKDAoFBAICAAUSAxMCCAoMCgUEAgIAARIDEwkNCgwKBQQC" + - "AgADEgMTEBEKCwoEBAICARIDFAITCgwKBQQCAgEFEgMUAggKDAoFBAICAQESAxQJDgoMCgUEAgIBAxID" + - "FBESCgoKAgQDEgQXABwBCgoKAwQDARIDFwgmCgsKBAQDAgASAxgCJwoMCgUEAwIABhIDGAIXCgwKBQQD" + - "AgABEgMYGCIKDAoFBAMCAAMSAxglJgoLCgQEAwIBEgMZAigKDAoFBAMCAQYSAxkCFwoMCgUEAwIBARID" + - "GRgjCgwKBQQDAgEDEgMZJicKCwoEBAMCAhIDGgImCgwKBQQDAgIGEgMaAhcKDAoFBAMCAgESAxoYIQoM" + - "CgUEAwICAxIDGiQlCgsKBAQDAgMSAxsCJwoMCgUEAwIDBhIDGwIXCgwKBQQDAgMBEgMbGCIKDAoFBAMC" + - "AwMSAxslJgoKCgIEBBIEHgAjAQoKCgMEBAESAx4IKQoLCgQEBAIAEgMfAhgKDAoFBAQCAAUSAx8CCAoM" + - "CgUEBAIAARIDHwkTCgwKBQQEAgADEgMfFhcKCwoEBAQCARIDIAIZCgwKBQQEAgEFEgMgAggKDAoFBAQC" + - "AQESAyAJFAoMCgUEBAIBAxIDIBcYCgsKBAQEAgISAyECFwoMCgUEBAICBRIDIQIICgwKBQQEAgIBEgMh" + - "CRIKDAoFBAQCAgMSAyEVFgoLCgQEBAIDEgMiAhgKDAoFBAQCAwUSAyICCAoMCgUEBAIDARIDIgkTCgwK" + - "BQQEAgMDEgMiFhcKCgoCBAUSBCUAKgEKCgoDBAUBEgMlCCoKCwoEBAUCABIDJgIYCgwKBQQFAgAFEgMm", - "AggKDAoFBAUCAAESAyYJEwoMCgUEBQIAAxIDJhYXCgsKBAQFAgESAycCGQoMCgUEBQIBBRIDJwIICgwK" + - "BQQFAgEBEgMnCRQKDAoFBAUCAQMSAycXGAoLCgQEBQICEgMoAhcKDAoFBAUCAgUSAygCCAoMCgUEBQIC" + - "ARIDKAkSCgwKBQQFAgIDEgMoFRYKCwoEBAUCAxIDKQIYCgwKBQQFAgMFEgMpAggKDAoFBAUCAwESAykJ" + - "EwoMCgUEBQIDAxIDKRYXCgoKAgQGEgQsADEBCgoKAwQGARIDLAgnCgsKBAQGAgASAy0CGAoMCgUEBgIA" + - "BRIDLQIICgwKBQQGAgABEgMtCRMKDAoFBAYCAAMSAy0WFwoLCgQEBgIBEgMuAhkKDAoFBAYCAQUSAy4C" + - "CAoMCgUEBgIBARIDLgkUCgwKBQQGAgEDEgMuFxgKCwoEBAYCAhIDLwIXCgwKBQQGAgIFEgMvAggKDAoF" + - "BAYCAgESAy8JEgoMCgUEBgICAxIDLxUWCgsKBAQGAgMSAzACGAoMCgUEBgIDBRIDMAIICgwKBQQGAgMB" + - "EgMwCRMKDAoFBAYCAwMSAzAWFwoKCgIEBxIEMwA1AQoKCgMEBwESAzMIJQoLCgQEBwIAEgM0Ai0KDAoF" + - "BAcCAAQSAzQCCgoMCgUEBwIABhIDNAsgCgwKBQQHAgABEgM0ISgKDAoFBAcCAAMSAzQrLAoKCgIECBIE" + - "NwA6AQoKCgMECAESAzcIJAoLCgQECAIAEgM4AhYKDAoFBAgCAAUSAzgCCAoMCgUECAIAARIDOAkRCgwK" + - "BQQIAgADEgM4FBUKCwoEBAgCARIDOQIfCgwKBQQIAgEGEgM5AhQKDAoFBAgCAQESAzkVGgoMCgUECAIB" + - "AxIDOR0eCgoKAgQJEgQ8AD8BCgoKAwQJARIDPAghCgsKBAQJAgASAz0CEwoMCgUECQIABRIDPQIICgwK" + - "BQQJAgABEgM9CQ4KDAoFBAkCAAMSAz0REgoLCgQECQIBEgM+Ah8KDAoFBAkCAQYSAz4CFAoMCgUECQIB" + - "ARIDPhUaCgwKBQQJAgEDEgM+HR5iBnByb3RvMw=="); + "CgRsZWZ0GAEgASgBUgRsZWZ0EhQKBXJpZ2h0GAIgASgBUgVyaWdodCJTCidQcm90b2J1ZkRpZmZlcmVu" + + "dGlhbERyaXZlV2hlZWxQb3NpdGlvbnMSEgoEbGVmdBgBIAEoAVIEbGVmdBIUCgVyaWdodBgCIAEoAVIF" + + "cmlnaHQipAIKHlByb3RvYnVmTWVjYW51bURyaXZlS2luZW1hdGljcxI/Cgpmcm9udF9sZWZ0GAEgASgL" + + "MiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJZnJvbnRMZWZ0EkEKC2Zyb250X3JpZ2h0" + + "GAIgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIKZnJvbnRSaWdodBI9CglyZWFy" + + "X2xlZnQYAyABKAsyIC53cGkucHJvdG8uUHJvdG9idWZUcmFuc2xhdGlvbjJkUghyZWFyTGVmdBI/Cgpy" + + "ZWFyX3JpZ2h0GAQgASgLMiAud3BpLnByb3RvLlByb3RvYnVmVHJhbnNsYXRpb24yZFIJcmVhclJpZ2h0" + + "Ip8BCiFQcm90b2J1Zk1lY2FudW1Ecml2ZU1vdG9yVm9sdGFnZXMSHQoKZnJvbnRfbGVmdBgBIAEoAVIJ" + + "ZnJvbnRMZWZ0Eh8KC2Zyb250X3JpZ2h0GAIgASgBUgpmcm9udFJpZ2h0EhsKCXJlYXJfbGVmdBgDIAEo" + + "AVIIcmVhckxlZnQSHQoKcmVhcl9yaWdodBgEIAEoAVIJcmVhclJpZ2h0IqABCiJQcm90b2J1Zk1lY2Fu" + + "dW1Ecml2ZVdoZWVsUG9zaXRpb25zEh0KCmZyb250X2xlZnQYASABKAFSCWZyb250TGVmdBIfCgtmcm9u" + + "dF9yaWdodBgCIAEoAVIKZnJvbnRSaWdodBIbCglyZWFyX2xlZnQYAyABKAFSCHJlYXJMZWZ0Eh0KCnJl" + + "YXJfcmlnaHQYBCABKAFSCXJlYXJSaWdodCKdAQofUHJvdG9idWZNZWNhbnVtRHJpdmVXaGVlbFNwZWVk" + + "cxIdCgpmcm9udF9sZWZ0GAEgASgBUglmcm9udExlZnQSHwoLZnJvbnRfcmlnaHQYAiABKAFSCmZyb250" + + "UmlnaHQSGwoJcmVhcl9sZWZ0GAMgASgBUghyZWFyTGVmdBIdCgpyZWFyX3JpZ2h0GAQgASgBUglyZWFy" + + "UmlnaHQiWwodUHJvdG9idWZTd2VydmVEcml2ZUtpbmVtYXRpY3MSOgoHbW9kdWxlcxgBIAMoCzIgLndw", + "aS5wcm90by5Qcm90b2J1ZlRyYW5zbGF0aW9uMmRSB21vZHVsZXMibwocUHJvdG9idWZTd2VydmVNb2R1" + + "bGVQb3NpdGlvbhIaCghkaXN0YW5jZRgBIAEoAVIIZGlzdGFuY2USMwoFYW5nbGUYAiABKAsyHS53cGku" + + "cHJvdG8uUHJvdG9idWZSb3RhdGlvbjJkUgVhbmdsZSJmChlQcm90b2J1ZlN3ZXJ2ZU1vZHVsZVN0YXRl" + + "EhQKBXNwZWVkGAEgASgBUgVzcGVlZBIzCgVhbmdsZRgCIAEoCzIdLndwaS5wcm90by5Qcm90b2J1ZlJv" + + "dGF0aW9uMmRSBWFuZ2xlQhoKGGVkdS53cGkuZmlyc3QubWF0aC5wcm90b0qNDwoGEgQAAEQBCggKAQwS" + + "AwAAEgoICgECEgMCABIKCQoCAwASAwQAGgoICgEIEgMGADEKCQoCCAESAwYAMQoKCgIEABIECAAMAQoK" + + "CgMEAAESAwgIHQoLCgQEAAIAEgMJAhAKDAoFBAACAAUSAwkCCAoMCgUEAAIAARIDCQkLCgwKBQQAAgAD" + + "EgMJDg8KCwoEBAACARIDCgIQCgwKBQQAAgEFEgMKAggKDAoFBAACAQESAwoJCwoMCgUEAAIBAxIDCg4P" + + "CgsKBAQAAgISAwsCEwoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCQ4KDAoFBAACAgMSAwsREgoKCgIE" + + "ARIEDgAQAQoKCgMEAQESAw4IKwoLCgQEAQIAEgMPAhkKDAoFBAECAAUSAw8CCAoMCgUEAQIAARIDDwkU" + + "CgwKBQQBAgADEgMPFxgKCgoCBAISBBIAFQEKCgoDBAIBEgMSCCwKCwoEBAICABIDEwISCgwKBQQCAgAF" + + "EgMTAggKDAoFBAICAAESAxMJDQoMCgUEAgIAAxIDExARCgsKBAQCAgESAxQCEwoMCgUEAgIBBRIDFAII" + + "CgwKBQQCAgEBEgMUCQ4KDAoFBAICAQMSAxQREgoKCgIEAxIEFwAaAQoKCgMEAwESAxcILwoLCgQEAwIA" + + "EgMYAhIKDAoFBAMCAAUSAxgCCAoMCgUEAwIAARIDGAkNCgwKBQQDAgADEgMYEBEKCwoEBAMCARIDGQIT" + + "CgwKBQQDAgEFEgMZAggKDAoFBAMCAQESAxkJDgoMCgUEAwIBAxIDGRESCgoKAgQEEgQcACEBCgoKAwQE" + + "ARIDHAgmCgsKBAQEAgASAx0CJwoMCgUEBAIABhIDHQIXCgwKBQQEAgABEgMdGCIKDAoFBAQCAAMSAx0l" + + "JgoLCgQEBAIBEgMeAigKDAoFBAQCAQYSAx4CFwoMCgUEBAIBARIDHhgjCgwKBQQEAgEDEgMeJicKCwoE" + + "BAQCAhIDHwImCgwKBQQEAgIGEgMfAhcKDAoFBAQCAgESAx8YIQoMCgUEBAICAxIDHyQlCgsKBAQEAgMS" + + "AyACJwoMCgUEBAIDBhIDIAIXCgwKBQQEAgMBEgMgGCIKDAoFBAQCAwMSAyAlJgoKCgIEBRIEIwAoAQoK" + + "CgMEBQESAyMIKQoLCgQEBQIAEgMkAhgKDAoFBAUCAAUSAyQCCAoMCgUEBQIAARIDJAkTCgwKBQQFAgAD", + "EgMkFhcKCwoEBAUCARIDJQIZCgwKBQQFAgEFEgMlAggKDAoFBAUCAQESAyUJFAoMCgUEBQIBAxIDJRcY" + + "CgsKBAQFAgISAyYCFwoMCgUEBQICBRIDJgIICgwKBQQFAgIBEgMmCRIKDAoFBAUCAgMSAyYVFgoLCgQE" + + "BQIDEgMnAhgKDAoFBAUCAwUSAycCCAoMCgUEBQIDARIDJwkTCgwKBQQFAgMDEgMnFhcKCgoCBAYSBCoA" + + "LwEKCgoDBAYBEgMqCCoKCwoEBAYCABIDKwIYCgwKBQQGAgAFEgMrAggKDAoFBAYCAAESAysJEwoMCgUE" + + "BgIAAxIDKxYXCgsKBAQGAgESAywCGQoMCgUEBgIBBRIDLAIICgwKBQQGAgEBEgMsCRQKDAoFBAYCAQMS" + + "AywXGAoLCgQEBgICEgMtAhcKDAoFBAYCAgUSAy0CCAoMCgUEBgICARIDLQkSCgwKBQQGAgIDEgMtFRYK" + + "CwoEBAYCAxIDLgIYCgwKBQQGAgMFEgMuAggKDAoFBAYCAwESAy4JEwoMCgUEBgIDAxIDLhYXCgoKAgQH" + + "EgQxADYBCgoKAwQHARIDMQgnCgsKBAQHAgASAzICGAoMCgUEBwIABRIDMgIICgwKBQQHAgABEgMyCRMK" + + "DAoFBAcCAAMSAzIWFwoLCgQEBwIBEgMzAhkKDAoFBAcCAQUSAzMCCAoMCgUEBwIBARIDMwkUCgwKBQQH" + + "AgEDEgMzFxgKCwoEBAcCAhIDNAIXCgwKBQQHAgIFEgM0AggKDAoFBAcCAgESAzQJEgoMCgUEBwICAxID" + + "NBUWCgsKBAQHAgMSAzUCGAoMCgUEBwIDBRIDNQIICgwKBQQHAgMBEgM1CRMKDAoFBAcCAwMSAzUWFwoK" + + "CgIECBIEOAA6AQoKCgMECAESAzgIJQoLCgQECAIAEgM5Ai0KDAoFBAgCAAQSAzkCCgoMCgUECAIABhID" + + "OQsgCgwKBQQIAgABEgM5ISgKDAoFBAgCAAMSAzkrLAoKCgIECRIEPAA/AQoKCgMECQESAzwIJAoLCgQE" + + "CQIAEgM9AhYKDAoFBAkCAAUSAz0CCAoMCgUECQIAARIDPQkRCgwKBQQJAgADEgM9FBUKCwoEBAkCARID" + + "PgIfCgwKBQQJAgEGEgM+AhQKDAoFBAkCAQESAz4VGgoMCgUECQIBAxIDPh0eCgoKAgQKEgRBAEQBCgoK" + + "AwQKARIDQQghCgsKBAQKAgASA0ICEwoMCgUECgIABRIDQgIICgwKBQQKAgABEgNCCQ4KDAoFBAoCAAMS" + + "A0IREgoLCgQECgIBEgNDAh8KDAoFBAoCAQYSA0MCFAoMCgUECgIBARIDQxUaCgwKBQQKAgEDEgNDHR5i" + + "BnByb3RvMw=="); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("kinematics.proto", "wpi.proto", descriptorData, Geometry2D.getDescriptor()); @@ -83,19 +87,21 @@ public final class Kinematics { static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelSpeeds_descriptor = descriptor.internalContainedType(200, 80, "ProtobufDifferentialDriveWheelSpeeds", "wpi.proto.ProtobufDifferentialDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(283, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor = descriptor.internalContainedType(282, 83, "ProtobufDifferentialDriveWheelPositions", "wpi.proto.ProtobufDifferentialDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveMotorVoltages_descriptor = descriptor.internalContainedType(578, 159, "ProtobufMecanumDriveMotorVoltages", "wpi.proto.ProtobufMecanumDriveMotorVoltages"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveKinematics_descriptor = descriptor.internalContainedType(368, 292, "ProtobufMecanumDriveKinematics", "wpi.proto.ProtobufMecanumDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(740, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveMotorVoltages_descriptor = descriptor.internalContainedType(663, 159, "ProtobufMecanumDriveMotorVoltages", "wpi.proto.ProtobufMecanumDriveMotorVoltages"); - static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(903, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelPositions_descriptor = descriptor.internalContainedType(825, 160, "ProtobufMecanumDriveWheelPositions", "wpi.proto.ProtobufMecanumDriveWheelPositions"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(1062, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); + static final Descriptors.Descriptor wpi_proto_ProtobufMecanumDriveWheelSpeeds_descriptor = descriptor.internalContainedType(988, 157, "ProtobufMecanumDriveWheelSpeeds", "wpi.proto.ProtobufMecanumDriveWheelSpeeds"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1155, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveDriveKinematics_descriptor = descriptor.internalContainedType(1147, 91, "ProtobufSwerveDriveKinematics", "wpi.proto.ProtobufSwerveDriveKinematics"); - static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1268, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModulePosition_descriptor = descriptor.internalContainedType(1240, 111, "ProtobufSwerveModulePosition", "wpi.proto.ProtobufSwerveModulePosition"); + + static final Descriptors.Descriptor wpi_proto_ProtobufSwerveModuleState_descriptor = descriptor.internalContainedType(1353, 102, "ProtobufSwerveModuleState", "wpi.proto.ProtobufSwerveModuleState"); /** * @return this proto file's descriptor. @@ -1113,6 +1119,344 @@ static class FieldNames { } } + /** + * Protobuf type {@code ProtobufDifferentialDriveWheelPositions} + */ + public static final class ProtobufDifferentialDriveWheelPositions extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional double left = 1; + */ + private double left; + + /** + * optional double right = 2; + */ + private double right; + + private ProtobufDifferentialDriveWheelPositions() { + } + + /** + * @return a new empty instance of {@code ProtobufDifferentialDriveWheelPositions} + */ + public static ProtobufDifferentialDriveWheelPositions newInstance() { + return new ProtobufDifferentialDriveWheelPositions(); + } + + /** + * optional double left = 1; + * @return whether the left field is set + */ + public boolean hasLeft() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional double left = 1; + * @return this + */ + public ProtobufDifferentialDriveWheelPositions clearLeft() { + bitField0_ &= ~0x00000001; + left = 0D; + return this; + } + + /** + * optional double left = 1; + * @return the left + */ + public double getLeft() { + return left; + } + + /** + * optional double left = 1; + * @param value the left to set + * @return this + */ + public ProtobufDifferentialDriveWheelPositions setLeft(final double value) { + bitField0_ |= 0x00000001; + left = value; + return this; + } + + /** + * optional double right = 2; + * @return whether the right field is set + */ + public boolean hasRight() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional double right = 2; + * @return this + */ + public ProtobufDifferentialDriveWheelPositions clearRight() { + bitField0_ &= ~0x00000002; + right = 0D; + return this; + } + + /** + * optional double right = 2; + * @return the right + */ + public double getRight() { + return right; + } + + /** + * optional double right = 2; + * @param value the right to set + * @return this + */ + public ProtobufDifferentialDriveWheelPositions setRight(final double value) { + bitField0_ |= 0x00000002; + right = value; + return this; + } + + @Override + public ProtobufDifferentialDriveWheelPositions copyFrom( + final ProtobufDifferentialDriveWheelPositions other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + left = other.left; + right = other.right; + } + return this; + } + + @Override + public ProtobufDifferentialDriveWheelPositions mergeFrom( + final ProtobufDifferentialDriveWheelPositions other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasLeft()) { + setLeft(other.left); + } + if (other.hasRight()) { + setRight(other.right); + } + return this; + } + + @Override + public ProtobufDifferentialDriveWheelPositions clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + left = 0D; + right = 0D; + return this; + } + + @Override + public ProtobufDifferentialDriveWheelPositions clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufDifferentialDriveWheelPositions)) { + return false; + } + ProtobufDifferentialDriveWheelPositions other = (ProtobufDifferentialDriveWheelPositions) o; + return bitField0_ == other.bitField0_ + && (!hasLeft() || ProtoUtil.isEqual(left, other.left)) + && (!hasRight() || ProtoUtil.isEqual(right, other.right)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 9); + output.writeDoubleNoTag(left); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 17); + output.writeDoubleNoTag(right); + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000002) != 0) { + size += 9; + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufDifferentialDriveWheelPositions mergeFrom(final ProtoSource input) throws + IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 9: { + // left + left = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 17) { + break; + } + } + case 17: { + // right + right = input.readDouble(); + bitField0_ |= 0x00000002; + tag = input.readTag(); + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000001) != 0) { + output.writeDouble(FieldNames.left, left); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeDouble(FieldNames.right, right); + } + output.endObject(); + } + + @Override + public ProtobufDifferentialDriveWheelPositions mergeFrom(final JsonSource input) throws + IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case 3317767: { + if (input.isAtField(FieldNames.left)) { + if (!input.trySkipNullValue()) { + left = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case 108511772: { + if (input.isAtField(FieldNames.right)) { + if (!input.trySkipNullValue()) { + right = input.readDouble(); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufDifferentialDriveWheelPositions clone() { + return new ProtobufDifferentialDriveWheelPositions().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufDifferentialDriveWheelPositions parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), data).checkInitialized(); + } + + public static ProtobufDifferentialDriveWheelPositions parseFrom(final ProtoSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), input).checkInitialized(); + } + + public static ProtobufDifferentialDriveWheelPositions parseFrom(final JsonSource input) throws + IOException { + return ProtoMessage.mergeFrom(new ProtobufDifferentialDriveWheelPositions(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufDifferentialDriveWheelPositions messages + */ + public static MessageFactory getFactory() { + return ProtobufDifferentialDriveWheelPositionsFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return Kinematics.wpi_proto_ProtobufDifferentialDriveWheelPositions_descriptor; + } + + private enum ProtobufDifferentialDriveWheelPositionsFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufDifferentialDriveWheelPositions create() { + return ProtobufDifferentialDriveWheelPositions.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName left = FieldName.forField("left"); + + static final FieldName right = FieldName.forField("right"); + } + } + /** * Protobuf type {@code ProtobufMecanumDriveKinematics} */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java index fbdf54d8818..d6036b8747b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java @@ -7,6 +7,8 @@ import static edu.wpi.first.units.Units.Meters; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelPositionsProto; +import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import java.util.Objects; @@ -20,6 +22,14 @@ public class DifferentialDriveWheelPositions /** Distance measured by the right side. */ public double rightMeters; + /** DifferentialDriveWheelPostions struct for serialization. */ + public static final DifferentialDriveWheelPositionsStruct struct = + new DifferentialDriveWheelPositionsStruct(); + + /** DifferentialDriveWheelPostions struct for serialization. */ + public static final DifferentialDriveWheelPositionsProto proto = + new DifferentialDriveWheelPositionsProto(); + /** * Constructs a DifferentialDriveWheelPositions. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java new file mode 100644 index 00000000000..982756b5b7c --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java @@ -0,0 +1,40 @@ +// 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. + +package edu.wpi.first.math.kinematics.proto; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; +import edu.wpi.first.math.proto.Kinematics.ProtobufDifferentialDriveWheelPositions; +import edu.wpi.first.util.protobuf.Protobuf; +import us.hebi.quickbuf.Descriptors.Descriptor; + +public class DifferentialDriveWheelPositionsProto + implements Protobuf { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelPositions.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufDifferentialDriveWheelPositions.getDescriptor(); + } + + @Override + public ProtobufDifferentialDriveWheelPositions createMessage() { + return ProtobufDifferentialDriveWheelPositions.newInstance(); + } + + @Override + public DifferentialDriveWheelPositions unpack(ProtobufDifferentialDriveWheelPositions msg) { + return new DifferentialDriveWheelPositions(msg.getLeft(), msg.getRight()); + } + + @Override + public void pack( + ProtobufDifferentialDriveWheelPositions msg, DifferentialDriveWheelPositions value) { + msg.setLeft(value.leftMeters); + msg.setRight(value.rightMeters); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java new file mode 100644 index 00000000000..efa80ebd177 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java @@ -0,0 +1,45 @@ +// 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. + +package edu.wpi.first.math.kinematics.struct; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; +import edu.wpi.first.util.struct.Struct; +import java.nio.ByteBuffer; + +public class DifferentialDriveWheelPositionsStruct + implements Struct { + @Override + public Class getTypeClass() { + return DifferentialDriveWheelPositions.class; + } + + @Override + public String getTypeString() { + return "struct:DifferentialDriveWheelPositions"; + } + + @Override + public int getSize() { + return kSizeDouble * 2; + } + + @Override + public String getSchema() { + return "double left;double right"; + } + + @Override + public DifferentialDriveWheelPositions unpack(ByteBuffer bb) { + double left = bb.getDouble(); + double right = bb.getDouble(); + return new DifferentialDriveWheelPositions(left, right); + } + + @Override + public void pack(ByteBuffer bb, DifferentialDriveWheelPositions value) { + bb.putDouble(value.leftMeters); + bb.putDouble(value.rightMeters); + } +} diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelPositionsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelPositionsProto.cpp new file mode 100644 index 00000000000..8a0af6f6fba --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelPositionsProto.cpp @@ -0,0 +1,34 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h" + +#include "kinematics.pb.h" + +google::protobuf::Message* wpi::Protobuf< + frc::DifferentialDriveWheelPositions>::New(google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + wpi::proto::ProtobufDifferentialDriveWheelPositions>(arena); +} + +frc::DifferentialDriveWheelPositions +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast( + &msg); + return frc::DifferentialDriveWheelPositions{ + units::meter_t{m->left()}, + units::meter_t{m->right()}, + }; +} + +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, + const frc::DifferentialDriveWheelPositions& value) { + auto m = + static_cast(msg); + m->set_left(value.left.value()); + m->set_right(value.right.value()); +} diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStruct.cpp new file mode 100644 index 00000000000..757ce62760c --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStruct.cpp @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h" + +namespace { +constexpr size_t kLeftOff = 0; +constexpr size_t kRightOff = kLeftOff + 8; +} // namespace + +using StructType = wpi::Struct; + +frc::DifferentialDriveWheelPositions StructType::Unpack( + std::span data) { + return frc::DifferentialDriveWheelPositions{ + units::meter_t{wpi::UnpackStruct(data)}, + units::meter_t{wpi::UnpackStruct(data)}, + }; +} + +void StructType::Pack(std::span data, + const frc::DifferentialDriveWheelPositions& value) { + wpi::PackStruct(data, value.left.value()); + wpi::PackStruct(data, value.right.value()); +} diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h index 4dddbeaf401..f22d6874f66 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h @@ -49,3 +49,6 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions { } }; } // namespace frc + +#include "frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h" +#include "frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h new file mode 100644 index 00000000000..3cf44eaa40c --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/proto/DifferentialDriveWheelPositionsProto.h @@ -0,0 +1,19 @@ +// 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. + +#pragma once + +#include +#include + +#include "frc/kinematics/DifferentialDriveWheelPositions.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static frc::DifferentialDriveWheelPositions Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const frc::DifferentialDriveWheelPositions& value); +}; diff --git a/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h new file mode 100644 index 00000000000..ef694bc37b4 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/struct/DifferentialDriveWheelPositionsStruct.h @@ -0,0 +1,28 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/kinematics/DifferentialDriveWheelPositions.h" + +template <> +struct WPILIB_DLLEXPORT wpi::Struct { + static constexpr std::string_view GetTypeString() { + return "struct:DifferentialDriveWheelPositions"; + } + static constexpr size_t GetSize() { return 16; } + static constexpr std::string_view GetSchema() { + return "double left;double right"; + } + + static frc::DifferentialDriveWheelPositions Unpack( + std::span data); + static void Pack(std::span data, + const frc::DifferentialDriveWheelPositions& value); +}; + +static_assert(wpi::StructSerializable); diff --git a/wpimath/src/main/proto/kinematics.proto b/wpimath/src/main/proto/kinematics.proto index 8fdbf2d8b05..4cb3b055a23 100644 --- a/wpimath/src/main/proto/kinematics.proto +++ b/wpimath/src/main/proto/kinematics.proto @@ -21,6 +21,11 @@ message ProtobufDifferentialDriveWheelSpeeds { double right = 2; } +message ProtobufDifferentialDriveWheelPositions { + double left = 1; + double right = 2; +} + message ProtobufMecanumDriveKinematics { ProtobufTranslation2d front_left = 1; ProtobufTranslation2d front_right = 2; diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java new file mode 100644 index 00000000000..84cc9967e1f --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics.struct; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import org.junit.jupiter.api.Test; + +class DifferentialDriveWheelPositionsStructTest { + private static final DifferentialDriveWheelPositions DATA = + new DifferentialDriveWheelPositions(1.74, 35.04); + + @Test + void testRoundtrip() { + ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelPositions.struct.getSize()); + buffer.order(ByteOrder.LITTLE_ENDIAN); + DifferentialDriveWheelPositions.struct.pack(buffer, DATA); + buffer.rewind(); + + DifferentialDriveWheelPositions data = DifferentialDriveWheelPositions.struct.unpack(buffer); + assertEquals(DATA.leftMeters, data.leftMeters); + assertEquals(DATA.rightMeters, data.rightMeters); + } +} diff --git a/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStructTest.cpp b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStructTest.cpp new file mode 100644 index 00000000000..59b52c25fba --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStructTest.cpp @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "frc/kinematics/DifferentialDriveWheelPositions.h" + +using namespace frc; + +namespace { + +using StructType = wpi::Struct; +const DifferentialDriveWheelPositions kExpectedData{ + DifferentialDriveWheelPositions{1.74_m, 35.04_m}}; +} // namespace + +TEST(DifferentialDriveWheelPositionsStructTest, Roundtrip) { + uint8_t buffer[StructType::GetSize()]; + std::memset(buffer, 0, StructType::GetSize()); + StructType::Pack(buffer, kExpectedData); + + DifferentialDriveWheelPositions unpacked_data = StructType::Unpack(buffer); + + EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value()); + EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value()); +}