Skip to content

Commit

Permalink
[wpimath] Move struct/proto classes to separate files (#5918)
Browse files Browse the repository at this point in the history 
Also add unit tests.
  • Loading branch information
@pjreiniger
pjreiniger authored Nov 21, 2023
1 parent 80d7ad5 commit 35744a0
Show file tree
Hide file tree
Showing 141 changed files with 3,360 additions and 1,379 deletions.
87 changes: 4 additions & 83 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,18 +10,15 @@
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.geometry.proto.Pose2dProto;
import edu.wpi.first.math.geometry.struct.Pose2dStruct;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.proto.Geometry2D.ProtobufPose2d;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.util.protobuf.Protobuf;
import edu.wpi.first.util.struct.Struct;
import java.nio.ByteBuffer;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;
import java.util.Objects;
import us.hebi.quickbuf.Descriptors.Descriptor;

/** Represents a 2D pose containing translational and rotational elements. */
@JsonIgnoreProperties(ignoreUnknown = true)
Expand Down Expand Up @@ -327,82 +324,6 @@ public Pose2d interpolate(Pose2d endValue, double t) {
}
}

public static final class AStruct implements Struct<Pose2d> {
@Override
public Class<Pose2d> getTypeClass() {
return Pose2d.class;
}

@Override
public String getTypeString() {
return "struct:Pose2d";
}

@Override
public int getSize() {
return Translation2d.struct.getSize() + Rotation2d.struct.getSize();
}

@Override
public String getSchema() {
return "Translation2d translation;Rotation2d rotation";
}

@Override
public Struct<?>[] getNested() {
return new Struct<?>[] {Translation2d.struct, Rotation2d.struct};
}

@Override
public Pose2d unpack(ByteBuffer bb) {
Translation2d translation = Translation2d.struct.unpack(bb);
Rotation2d rotation = Rotation2d.struct.unpack(bb);
return new Pose2d(translation, rotation);
}

@Override
public void pack(ByteBuffer bb, Pose2d value) {
Translation2d.struct.pack(bb, value.m_translation);
Rotation2d.struct.pack(bb, value.m_rotation);
}
}

public static final AStruct struct = new AStruct();

public static final class AProto implements Protobuf<Pose2d, ProtobufPose2d> {
@Override
public Class<Pose2d> getTypeClass() {
return Pose2d.class;
}

@Override
public Descriptor getDescriptor() {
return ProtobufPose2d.getDescriptor();
}

@Override
public Protobuf<?, ?>[] getNested() {
return new Protobuf<?, ?>[] {Translation2d.proto, Rotation2d.proto};
}

@Override
public ProtobufPose2d createMessage() {
return ProtobufPose2d.newInstance();
}

@Override
public Pose2d unpack(ProtobufPose2d msg) {
return new Pose2d(
Translation2d.proto.unpack(msg.getTranslation()),
Rotation2d.proto.unpack(msg.getRotation()));
}

@Override
public void pack(ProtobufPose2d msg, Pose2d value) {
Translation2d.proto.pack(msg.getMutableTranslation(), value.m_translation);
Rotation2d.proto.pack(msg.getMutableRotation(), value.m_rotation);
}
}

public static final AProto proto = new AProto();
public static final Pose2dStruct struct = new Pose2dStruct();
public static final Pose2dProto proto = new Pose2dProto();
}
87 changes: 4 additions & 83 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,13 +9,10 @@
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.math.geometry.proto.Pose3dProto;
import edu.wpi.first.math.geometry.struct.Pose3dStruct;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.proto.Geometry3D.ProtobufPose3d;
import edu.wpi.first.util.protobuf.Protobuf;
import edu.wpi.first.util.struct.Struct;
import java.nio.ByteBuffer;
import java.util.Objects;
import us.hebi.quickbuf.Descriptors.Descriptor;

/** Represents a 3D pose containing translational and rotational elements. */
@JsonIgnoreProperties(ignoreUnknown = true)
Expand Down Expand Up @@ -324,82 +321,6 @@ public Pose3d interpolate(Pose3d endValue, double t) {
}
}

public static final class AStruct implements Struct<Pose3d> {
@Override
public Class<Pose3d> getTypeClass() {
return Pose3d.class;
}

@Override
public String getTypeString() {
return "struct:Pose3d";
}

@Override
public int getSize() {
return Translation3d.struct.getSize() + Rotation3d.struct.getSize();
}

@Override
public String getSchema() {
return "Translation3d translation;Rotation3d rotation";
}

@Override
public Struct<?>[] getNested() {
return new Struct<?>[] {Translation3d.struct, Rotation3d.struct};
}

@Override
public Pose3d unpack(ByteBuffer bb) {
Translation3d translation = Translation3d.struct.unpack(bb);
Rotation3d rotation = Rotation3d.struct.unpack(bb);
return new Pose3d(translation, rotation);
}

@Override
public void pack(ByteBuffer bb, Pose3d value) {
Translation3d.struct.pack(bb, value.m_translation);
Rotation3d.struct.pack(bb, value.m_rotation);
}
}

public static final AStruct struct = new AStruct();

public static final class AProto implements Protobuf<Pose3d, ProtobufPose3d> {
@Override
public Class<Pose3d> getTypeClass() {
return Pose3d.class;
}

@Override
public Descriptor getDescriptor() {
return ProtobufPose3d.getDescriptor();
}

@Override
public Protobuf<?, ?>[] getNested() {
return new Protobuf<?, ?>[] {Translation3d.proto, Rotation3d.proto};
}

@Override
public ProtobufPose3d createMessage() {
return ProtobufPose3d.newInstance();
}

@Override
public Pose3d unpack(ProtobufPose3d msg) {
return new Pose3d(
Translation3d.proto.unpack(msg.getTranslation()),
Rotation3d.proto.unpack(msg.getRotation()));
}

@Override
public void pack(ProtobufPose3d msg, Pose3d value) {
Translation3d.proto.pack(msg.getMutableTranslation(), value.m_translation);
Rotation3d.proto.pack(msg.getMutableRotation(), value.m_rotation);
}
}

public static final AProto proto = new AProto();
public static final Pose3dStruct struct = new Pose3dStruct();
public static final Pose3dProto proto = new Pose3dProto();
}
78 changes: 4 additions & 74 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,13 +10,10 @@
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.geometry.proto.QuaternionProto;
import edu.wpi.first.math.geometry.struct.QuaternionStruct;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.proto.Geometry3D.ProtobufQuaternion;
import edu.wpi.first.util.protobuf.Protobuf;
import edu.wpi.first.util.struct.Struct;
import java.nio.ByteBuffer;
import java.util.Objects;
import us.hebi.quickbuf.Descriptors.Descriptor;

@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
Expand Down Expand Up @@ -406,73 +403,6 @@ public Vector<N3> toRotationVector() {
return VecBuilder.fill(coeff * getX(), coeff * getY(), coeff * getZ());
}

public static final class AStruct implements Struct<Quaternion> {
@Override
public Class<Quaternion> getTypeClass() {
return Quaternion.class;
}

@Override
public String getTypeString() {
return "struct:Quaternion";
}

@Override
public int getSize() {
return kSizeDouble * 4;
}

@Override
public String getSchema() {
return "double w;double x;double y;double z";
}

@Override
public Quaternion unpack(ByteBuffer bb) {
double w = bb.getDouble();
double x = bb.getDouble();
double y = bb.getDouble();
double z = bb.getDouble();
return new Quaternion(w, x, y, z);
}

@Override
public void pack(ByteBuffer bb, Quaternion value) {
bb.putDouble(value.getW());
bb.putDouble(value.getX());
bb.putDouble(value.getY());
bb.putDouble(value.getZ());
}
}

public static final AStruct struct = new AStruct();

public static final class AProto implements Protobuf<Quaternion, ProtobufQuaternion> {
@Override
public Class<Quaternion> getTypeClass() {
return Quaternion.class;
}

@Override
public Descriptor getDescriptor() {
return ProtobufQuaternion.getDescriptor();
}

@Override
public ProtobufQuaternion createMessage() {
return ProtobufQuaternion.newInstance();
}

@Override
public Quaternion unpack(ProtobufQuaternion msg) {
return new Quaternion(msg.getW(), msg.getX(), msg.getY(), msg.getZ());
}

@Override
public void pack(ProtobufQuaternion msg, Quaternion value) {
msg.setW(value.getW()).setX(value.getX()).setY(value.getY()).setZ(value.getZ());
}
}

public static final AProto proto = new AProto();
public static final QuaternionStruct struct = new QuaternionStruct();
public static final QuaternionProto proto = new QuaternionProto();
}
71 changes: 4 additions & 67 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,16 +11,13 @@
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.proto.Rotation2dProto;
import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.proto.Geometry2D.ProtobufRotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.util.protobuf.Protobuf;
import edu.wpi.first.util.struct.Struct;
import java.nio.ByteBuffer;
import java.util.Objects;
import us.hebi.quickbuf.Descriptors.Descriptor;

/**
* A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Expand Down Expand Up @@ -275,66 +272,6 @@ public Rotation2d interpolate(Rotation2d endValue, double t) {
return plus(endValue.minus(this).times(MathUtil.clamp(t, 0, 1)));
}

public static final class AStruct implements Struct<Rotation2d> {
@Override
public Class<Rotation2d> getTypeClass() {
return Rotation2d.class;
}

@Override
public String getTypeString() {
return "struct:Rotation2d";
}

@Override
public int getSize() {
return kSizeDouble;
}

@Override
public String getSchema() {
return "double value";
}

@Override
public Rotation2d unpack(ByteBuffer bb) {
return new Rotation2d(bb.getDouble());
}

@Override
public void pack(ByteBuffer bb, Rotation2d value) {
bb.putDouble(value.m_value);
}
}

public static final AStruct struct = new AStruct();

public static final class AProto implements Protobuf<Rotation2d, ProtobufRotation2d> {
@Override
public Class<Rotation2d> getTypeClass() {
return Rotation2d.class;
}

@Override
public Descriptor getDescriptor() {
return ProtobufRotation2d.getDescriptor();
}

@Override
public ProtobufRotation2d createMessage() {
return ProtobufRotation2d.newInstance();
}

@Override
public Rotation2d unpack(ProtobufRotation2d msg) {
return new Rotation2d(msg.getValue());
}

@Override
public void pack(ProtobufRotation2d msg, Rotation2d value) {
msg.setValue(value.m_value);
}
}

public static final AProto proto = new AProto();
public static final Rotation2dStruct struct = new Rotation2dStruct();
public static final Rotation2dProto proto = new Rotation2dProto();
}
Loading

0 comments on commit 35744a0

Please sign in to comment.