Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Also add unit tests.
This commit is contained in:
PJ Reiniger
2023-11-21 13:11:57 -05:00
committed by GitHub
parent 80d7ad58ea
commit 35744a036e
141 changed files with 3360 additions and 1379 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose2dProtoTest.java Normal file
View File

@@ -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.
package edu.wpi.first.math.geometry.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d;
import org.junit.jupiter.api.Test;
class Pose2dProtoTest {
private static final Pose2d DATA =
new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9));
@Test
void testRoundtrip() {
ProtobufPose2d proto = Pose2d.proto.createMessage();
Pose2d.proto.pack(proto, DATA);
Pose2d data = Pose2d.proto.unpack(proto);
assertEquals(DATA.getTranslation(), data.getTranslation());
assertEquals(DATA.getRotation(), data.getRotation());
}
}

31
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Pose3dProtoTest.java Normal file
View File

@@ -0,0 +1,31 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d;
import org.junit.jupiter.api.Test;
class Pose3dProtoTest {
private static final Pose3d DATA =
new Pose3d(
new Translation3d(1.1, 2.2, 1.1),
new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74)));
@Test
void testRoundtrip() {
ProtobufPose3d proto = Pose3d.proto.createMessage();
Pose3d.proto.pack(proto, DATA);
Pose3d data = Pose3d.proto.unpack(proto);
assertEquals(DATA.getTranslation(), data.getTranslation());
assertEquals(DATA.getRotation(), data.getRotation());
}
}

27
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/QuaternionProtoTest.java Normal file
View File

@@ -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.
package edu.wpi.first.math.geometry.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion;
import org.junit.jupiter.api.Test;
class QuaternionProtoTest {
private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1);
@Test
void testRoundtrip() {
ProtobufQuaternion proto = Quaternion.proto.createMessage();
Quaternion.proto.pack(proto, DATA);
Quaternion data = Quaternion.proto.unpack(proto);
assertEquals(DATA.getW(), data.getW());
assertEquals(DATA.getX(), data.getX());
assertEquals(DATA.getY(), data.getY());
assertEquals(DATA.getZ(), data.getZ());
}
}

24
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation2dProtoTest.java Normal file
View File

@@ -0,0 +1,24 @@
// 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.geometry.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d;
import org.junit.jupiter.api.Test;
class Rotation2dProtoTest {
private static final Rotation2d DATA = new Rotation2d(1.91);
@Test
void testRoundtrip() {
ProtobufRotation2d proto = Rotation2d.proto.createMessage();
Rotation2d.proto.pack(proto, DATA);
Rotation2d data = Rotation2d.proto.unpack(proto);
assertEquals(DATA.getRadians(), data.getRadians());
}
}

25
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Rotation3dProtoTest.java Normal file
View File

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.proto.Geometry3D.ProtobufRotation3d;
import org.junit.jupiter.api.Test;
class Rotation3dProtoTest {
private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4));
@Test
void testRoundtrip() {
ProtobufRotation3d proto = Rotation3d.proto.createMessage();
Rotation3d.proto.pack(proto, DATA);
Rotation3d data = Rotation3d.proto.unpack(proto);
assertEquals(DATA.getQuaternion(), data.getQuaternion());
}
}

28
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform2dProtoTest.java Normal file
View File

@@ -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.
package edu.wpi.first.math.geometry.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.proto.Geometry2D.ProtobufTransform2d;
import org.junit.jupiter.api.Test;
class Transform2dProtoTest {
private static final Transform2d DATA =
new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4));
@Test
void testRoundtrip() {
ProtobufTransform2d proto = Transform2d.proto.createMessage();
Transform2d.proto.pack(proto, DATA);
Transform2d data = Transform2d.proto.unpack(proto);
assertEquals(DATA.getTranslation(), data.getTranslation());
assertEquals(DATA.getRotation(), data.getRotation());
}
}

31
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Transform3dProtoTest.java Normal file
View File

@@ -0,0 +1,31 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.proto.Geometry3D.ProtobufTransform3d;
import org.junit.jupiter.api.Test;
class Transform3dProtoTest {
private static final Transform3d DATA =
new Transform3d(
new Translation3d(0.3504, 22.9, 3.504),
new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504)));
@Test
void testRoundtrip() {
ProtobufTransform3d proto = Transform3d.proto.createMessage();
Transform3d.proto.pack(proto, DATA);
Transform3d data = Transform3d.proto.unpack(proto);
assertEquals(DATA.getTranslation(), data.getTranslation());
assertEquals(DATA.getRotation(), data.getRotation());
}
}

25
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation2dProtoTest.java Normal file
View File

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.proto.Geometry2D.ProtobufTranslation2d;
import org.junit.jupiter.api.Test;
class Translation2dProtoTest {
private static final Translation2d DATA = new Translation2d(3.504, 22.9);
@Test
void testRoundtrip() {
ProtobufTranslation2d proto = Translation2d.proto.createMessage();
Translation2d.proto.pack(proto, DATA);
Translation2d data = Translation2d.proto.unpack(proto);
assertEquals(DATA.getX(), data.getX());
assertEquals(DATA.getY(), data.getY());
}
}

26
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Translation3dProtoTest.java Normal file
View File

@@ -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.
package edu.wpi.first.math.geometry.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.proto.Geometry3D.ProtobufTranslation3d;
import org.junit.jupiter.api.Test;
class Translation3dProtoTest {
private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504);
@Test
void testRoundtrip() {
ProtobufTranslation3d proto = Translation3d.proto.createMessage();
Translation3d.proto.pack(proto, DATA);
Translation3d data = Translation3d.proto.unpack(proto);
assertEquals(DATA.getX(), data.getX());
assertEquals(DATA.getY(), data.getY());
assertEquals(DATA.getZ(), data.getZ());
}
}

26
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist2dProtoTest.java Normal file
View File

@@ -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.
package edu.wpi.first.math.geometry.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.proto.Geometry2D.ProtobufTwist2d;
import org.junit.jupiter.api.Test;
class Twist2dProtoTest {
private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04);
@Test
void testRoundtrip() {
ProtobufTwist2d proto = Twist2d.proto.createMessage();
Twist2d.proto.pack(proto, DATA);
Twist2d data = Twist2d.proto.unpack(proto);
assertEquals(DATA.dx, data.dx);
assertEquals(DATA.dy, data.dy);
assertEquals(DATA.dtheta, data.dtheta);
}
}

29
wpimath/src/test/java/edu/wpi/first/math/geometry/proto/Twist3dProtoTest.java Normal file
View File

@@ -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.geometry.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Twist3d;
import edu.wpi.first.math.proto.Geometry3D.ProtobufTwist3d;
import org.junit.jupiter.api.Test;
class Twist3dProtoTest {
private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4);
@Test
void testRoundtrip() {
ProtobufTwist3d proto = Twist3d.proto.createMessage();
Twist3d.proto.pack(proto, DATA);
Twist3d data = Twist3d.proto.unpack(proto);
assertEquals(DATA.dx, data.dx);
assertEquals(DATA.dy, data.dy);
assertEquals(DATA.dz, data.dz);
assertEquals(DATA.rx, data.rx);
assertEquals(DATA.ry, data.ry);
assertEquals(DATA.rz, data.rz);
}
}

31
wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose2dStructTest.java Normal file
View File

@@ -0,0 +1,31 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
class Pose2dStructTest {
private static final Pose2d DATA =
new Pose2d(new Translation2d(0.191, 2.2), new Rotation2d(22.9));
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(Pose2d.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
Pose2d.struct.pack(buffer, DATA);
buffer.rewind();
Pose2d data = Pose2d.struct.unpack(buffer);
assertEquals(DATA.getTranslation(), data.getTranslation());
assertEquals(DATA.getRotation(), data.getRotation());
}
}

34
wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Pose3dStructTest.java Normal file
View File

@@ -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.
package edu.wpi.first.math.geometry.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
class Pose3dStructTest {
private static final Pose3d DATA =
new Pose3d(
new Translation3d(1.1, 2.2, 1.1),
new Rotation3d(new Quaternion(1.91, 0.3504, 3.3, 1.74)));
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(Pose3d.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
Pose3d.struct.pack(buffer, DATA);
buffer.rewind();
Pose3d data = Pose3d.struct.unpack(buffer);
assertEquals(DATA.getTranslation(), data.getTranslation());
assertEquals(DATA.getRotation(), data.getRotation());
}
}

30
wpimath/src/test/java/edu/wpi/first/math/geometry/struct/QuaternionStructTest.java Normal file
View File

@@ -0,0 +1,30 @@
// 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.geometry.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Quaternion;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
class QuaternionStructTest {
private static final Quaternion DATA = new Quaternion(1.1, 0.191, 35.04, 19.1);
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(Quaternion.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
Quaternion.struct.pack(buffer, DATA);
buffer.rewind();
Quaternion data = Quaternion.struct.unpack(buffer);
assertEquals(DATA.getW(), data.getW());
assertEquals(DATA.getX(), data.getX());
assertEquals(DATA.getY(), data.getY());
assertEquals(DATA.getZ(), data.getZ());
}
}

27
wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation2dStructTest.java Normal file
View File

@@ -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.
package edu.wpi.first.math.geometry.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Rotation2d;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
class Rotation2dStructTest {
private static final Rotation2d DATA = new Rotation2d(1.91);
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(Rotation2d.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
Rotation2d.struct.pack(buffer, DATA);
buffer.rewind();
Rotation2d data = Rotation2d.struct.unpack(buffer);
assertEquals(DATA.getRadians(), data.getRadians());
}
}

28
wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Rotation3dStructTest.java Normal file
View File

@@ -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.
package edu.wpi.first.math.geometry.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
class Rotation3dStructTest {
private static final Rotation3d DATA = new Rotation3d(new Quaternion(2.29, 0.191, 0.191, 17.4));
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(Rotation3d.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
Rotation3d.struct.pack(buffer, DATA);
buffer.rewind();
Rotation3d data = Rotation3d.struct.unpack(buffer);
assertEquals(DATA.getQuaternion(), data.getQuaternion());
}
}

31
wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform2dStructTest.java Normal file
View File

@@ -0,0 +1,31 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
class Transform2dStructTest {
private static final Transform2d DATA =
new Transform2d(new Translation2d(0.191, 2.2), new Rotation2d(4.4));
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(Transform2d.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
Transform2d.struct.pack(buffer, DATA);
buffer.rewind();
Transform2d data = Transform2d.struct.unpack(buffer);
assertEquals(DATA.getTranslation(), data.getTranslation());
assertEquals(DATA.getRotation(), data.getRotation());
}
}

34
wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Transform3dStructTest.java Normal file
View File

@@ -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.
package edu.wpi.first.math.geometry.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
class Transform3dStructTest {
private static final Transform3d DATA =
new Transform3d(
new Translation3d(0.3504, 22.9, 3.504),
new Rotation3d(new Quaternion(0.3504, 35.04, 2.29, 0.3504)));
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(Transform3d.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
Transform3d.struct.pack(buffer, DATA);
buffer.rewind();
Transform3d data = Transform3d.struct.unpack(buffer);
assertEquals(DATA.getTranslation(), data.getTranslation());
assertEquals(DATA.getRotation(), data.getRotation());
}
}

28
wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation2dStructTest.java Normal file
View File

@@ -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.
package edu.wpi.first.math.geometry.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Translation2d;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
class Translation2dStructTest {
private static final Translation2d DATA = new Translation2d(3.504, 22.9);
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(Translation2d.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
Translation2d.struct.pack(buffer, DATA);
buffer.rewind();
Translation2d data = Translation2d.struct.unpack(buffer);
assertEquals(DATA.getX(), data.getX());
assertEquals(DATA.getY(), data.getY());
}
}

29
wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Translation3dStructTest.java Normal file
View File

@@ -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.geometry.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Translation3d;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
class Translation3dStructTest {
private static final Translation3d DATA = new Translation3d(35.04, 22.9, 3.504);
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(Translation3d.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
Translation3d.struct.pack(buffer, DATA);
buffer.rewind();
Translation3d data = Translation3d.struct.unpack(buffer);
assertEquals(DATA.getX(), data.getX());
assertEquals(DATA.getY(), data.getY());
assertEquals(DATA.getZ(), data.getZ());
}
}

29
wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist2dStructTest.java Normal file
View File

@@ -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.geometry.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Twist2d;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
class Twist2dStructTest {
private static final Twist2d DATA = new Twist2d(2.29, 35.04, 35.04);
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(Twist2d.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
Twist2d.struct.pack(buffer, DATA);
buffer.rewind();
Twist2d data = Twist2d.struct.unpack(buffer);
assertEquals(DATA.dx, data.dx);
assertEquals(DATA.dy, data.dy);
assertEquals(DATA.dtheta, data.dtheta);
}
}

32
wpimath/src/test/java/edu/wpi/first/math/geometry/struct/Twist3dStructTest.java Normal file
View File

@@ -0,0 +1,32 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Twist3d;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
class Twist3dStructTest {
private static final Twist3d DATA = new Twist3d(1.1, 2.29, 35.04, 0.174, 19.1, 4.4);
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(Twist3d.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
Twist3d.struct.pack(buffer, DATA);
buffer.rewind();
Twist3d data = Twist3d.struct.unpack(buffer);
assertEquals(DATA.dx, data.dx);
assertEquals(DATA.dy, data.dy);
assertEquals(DATA.dz, data.dz);
assertEquals(DATA.rx, data.rx);
assertEquals(DATA.ry, data.ry);
assertEquals(DATA.rz, data.rz);
}
}