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

Add PacketSerde interface and expand PacketUtils for more wpimath classes (#1058)

Browse Source 
Follows a similar system to the current Protobuf implementation that helps make code more readable and expandable. Also wraps the NT topic to be more useful. Impl stuff is hidden so it can't be extended. Optimizes AT-specific classes by only serializing data when needed, won't save on size but will on time.

closes #1003
This commit is contained in:
Sriman Achanta
2023-12-24 19:56:08 -05:00
committed by GitHub
parent 0356eeeb50
commit 5be9b8be2c
17 changed files with 792 additions and 433 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java Normal file
View File

@@ -0,0 +1,26 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.common.dataflow.structures;
public interface PacketSerde<T> {
int getMaxByteSize();
void pack(Packet packet, T value);
T unpack(Packet packet);
}

11
photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java
View File

@@ -27,7 +27,7 @@ import edu.wpi.first.networktables.IntegerSubscriber;
import edu.wpi.first.networktables.IntegerTopic;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.RawPublisher;
import org.photonvision.targeting.PhotonPipelineResult;
/**
* This class is a wrapper around all per-pipeline NT topics that PhotonVision should be publishing
@@ -39,7 +39,8 @@ import edu.wpi.first.networktables.RawPublisher;
*/
public class NTTopicSet {
public NetworkTable subTable;
public RawPublisher rawBytesEntry;
public PacketPublisher<PhotonPipelineResult> resultPublisher;
public IntegerPublisher pipelineIndexPublisher;
public IntegerSubscriber pipelineIndexRequestSub;
@@ -69,11 +70,13 @@ public class NTTopicSet {
public DoubleArrayPublisher cameraDistortionPublisher;
public void updateEntries() {
rawBytesEntry =
var rawBytesEntry =
subTable
.getRawTopic("rawBytes")
.publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
resultPublisher = new PacketPublisher<>(rawBytesEntry, PhotonPipelineResult.serde);
pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish();
pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0);
@@ -104,7 +107,7 @@ public class NTTopicSet {
@SuppressWarnings("DuplicatedCode")
public void removeEntries() {
if (rawBytesEntry != null) rawBytesEntry.close();
if (resultPublisher != null) resultPublisher.close();
if (pipelineIndexPublisher != null) pipelineIndexPublisher.close();
if (pipelineIndexRequestSub != null) pipelineIndexRequestSub.close();

47
photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java Normal file
View File

@@ -0,0 +1,47 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.common.networktables;
import edu.wpi.first.networktables.RawPublisher;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
public class PacketPublisher<T> implements AutoCloseable {
public final RawPublisher publisher;
private final PacketSerde<T> serde;
public PacketPublisher(RawPublisher publisher, PacketSerde<T> serde) {
this.publisher = publisher;
this.serde = serde;
}
public void accept(T value, int byteSize) {
var packet = new Packet(byteSize);
serde.pack(packet, value);
publisher.set(packet.getData());
}
public void accept(T value) {
accept(value, serde.getMaxByteSize());
}
@Override
public void close() {
publisher.close();
}
}

50
photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java Normal file
View File

@@ -0,0 +1,50 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.common.networktables;
import edu.wpi.first.networktables.RawSubscriber;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
public class PacketSubscriber<T> implements AutoCloseable {
public final RawSubscriber subscriber;
private final PacketSerde<T> serde;
private final T defaultValue;
private final Packet packet = new Packet(1);
public PacketSubscriber(RawSubscriber subscriber, PacketSerde<T> serde, T defaultValue) {
this.subscriber = subscriber;
this.serde = serde;
this.defaultValue = defaultValue;
}
public T get() {
packet.clear();
packet.setData(subscriber.get(new byte[] {}));
if (packet.getSize() < 1) return defaultValue;
return serde.unpack(packet);
}
@Override
public void close() {
subscriber.close();
}
}

59
photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java
View File

@@ -20,12 +20,11 @@ package org.photonvision.targeting;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
public class MultiTargetPNPResult {
// Seeing 32 apriltags at once seems like a sane limit
private static final int MAX_IDS = 32;
// pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally)
public static final int PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS);
public PNPResult estimatedPose = new PNPResult();
public List<Integer> fiducialIDsUsed = List.of();
@@ -37,27 +36,6 @@ public class MultiTargetPNPResult {
fiducialIDsUsed = ids;
}
public static MultiTargetPNPResult createFromPacket(Packet packet) {
var results = PNPResult.createFromPacket(packet);
var ids = new ArrayList<Integer>(MAX_IDS);
for (int i = 0; i < MAX_IDS; i++) {
int targetId = packet.decodeShort();
if (targetId > -1) ids.add(targetId);
}
return new MultiTargetPNPResult(results, ids);
}
public void populatePacket(Packet packet) {
estimatedPose.populatePacket(packet);
for (int i = 0; i < MAX_IDS; i++) {
if (i < fiducialIDsUsed.size()) {
packet.encode((short) fiducialIDsUsed.get(i).byteValue());
} else {
packet.encode((short) -1);
}
}
}
@Override
public int hashCode() {
final int prime = 31;
@@ -90,4 +68,39 @@ public class MultiTargetPNPResult {
+ fiducialIDsUsed
+ "]";
}
public static final class APacketSerde implements PacketSerde<MultiTargetPNPResult> {
@Override
public int getMaxByteSize() {
// PNPResult + MAX_IDS possible targets (arbitrary upper limit that should never be hit,
// ideally)
return PNPResult.serde.getMaxByteSize() + (Short.BYTES * MAX_IDS);
}
@Override
public void pack(Packet packet, MultiTargetPNPResult result) {
PNPResult.serde.pack(packet, result.estimatedPose);
for (int i = 0; i < MAX_IDS; i++) {
if (i < result.fiducialIDsUsed.size()) {
packet.encode((short) result.fiducialIDsUsed.get(i).byteValue());
} else {
packet.encode((short) -1);
}
}
}
@Override
public MultiTargetPNPResult unpack(Packet packet) {
var results = PNPResult.serde.unpack(packet);
var ids = new ArrayList<Integer>(MAX_IDS);
for (int i = 0; i < MAX_IDS; i++) {
int targetId = packet.decodeShort();
if (targetId > -1) ids.add(targetId);
}
return new MultiTargetPNPResult(results, ids);
}
}
public static final APacketSerde serde = new APacketSerde();
}

64
photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java
View File

@@ -19,6 +19,7 @@ package org.photonvision.targeting;
import edu.wpi.first.math.geometry.Transform3d;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.utils.PacketUtils;
/**
@@ -86,31 +87,6 @@ public class PNPResult {
this.altReprojErr = altReprojErr;
}
public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3);
public static PNPResult createFromPacket(Packet packet) {
var present = packet.decodeBoolean();
var best = PacketUtils.decodeTransform(packet);
var alt = PacketUtils.decodeTransform(packet);
var bestEr = packet.decodeDouble();
var altEr = packet.decodeDouble();
var ambiguity = packet.decodeDouble();
if (present) {
return new PNPResult(best, alt, ambiguity, bestEr, altEr);
} else {
return new PNPResult();
}
}
public void populatePacket(Packet packet) {
packet.encode(isPresent);
PacketUtils.encodeTransform(packet, best);
PacketUtils.encodeTransform(packet, alt);
packet.encode(bestReprojErr);
packet.encode(altReprojErr);
packet.encode(ambiguity);
}
@Override
public int hashCode() {
final int prime = 31;
@@ -166,4 +142,42 @@ public class PNPResult {
+ ambiguity
+ "]";
}
public static final class APacketSerde implements PacketSerde<PNPResult> {
@Override
public int getMaxByteSize() {
return 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3);
}
@Override
public void pack(Packet packet, PNPResult value) {
packet.encode(value.isPresent);
if (value.isPresent) {
PacketUtils.packTransform3d(packet, value.best);
PacketUtils.packTransform3d(packet, value.alt);
packet.encode(value.bestReprojErr);
packet.encode(value.altReprojErr);
packet.encode(value.ambiguity);
}
}
@Override
public PNPResult unpack(Packet packet) {
var present = packet.decodeBoolean();
if (!present) {
return new PNPResult();
}
var best = PacketUtils.unpackTransform3d(packet);
var alt = PacketUtils.unpackTransform3d(packet);
var bestEr = packet.decodeDouble();
var altEr = packet.decodeDouble();
var ambiguity = packet.decodeDouble();
return new PNPResult(best, alt, ambiguity, bestEr, altEr);
}
}
public static final APacketSerde serde = new APacketSerde();
}

83
photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
View File

@@ -20,6 +20,7 @@ package org.photonvision.targeting;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
/** Represents a pipeline result from a PhotonCamera. */
public class PhotonPipelineResult {
@@ -71,10 +72,10 @@ public class PhotonPipelineResult {
* @return The size of the packet needed to store this pipeline result.
*/
public int getPacketSize() {
return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES
+ 8 // latency
+ MultiTargetPNPResult.PACK_SIZE_BYTES
+ 1; // target count
return Double.BYTES // latency
+ 1 // target count
+ targets.size() * PhotonTrackedTarget.serde.getMaxByteSize()
+ MultiTargetPNPResult.serde.getMaxByteSize();
}
/**
@@ -150,49 +151,6 @@ public class PhotonPipelineResult {
return multiTagResult;
}
/**
* Populates the fields of the pipeline result from the packet.
*
* @param packet The incoming packet.
* @return The incoming packet.
*/
public Packet createFromPacket(Packet packet) {
// Decode latency, existence of targets, and number of targets.
latencyMillis = packet.decodeDouble();
this.multiTagResult = MultiTargetPNPResult.createFromPacket(packet);
byte targetCount = packet.decodeByte();
targets.clear();
// Decode the information of each target.
for (int i = 0; i < (int) targetCount; ++i) {
var target = new PhotonTrackedTarget();
target.createFromPacket(packet);
targets.add(target);
}
return packet;
}
/**
* Populates the outgoing packet with information from this pipeline result.
*
* @param packet The outgoing packet.
* @return The outgoing packet.
*/
public Packet populatePacket(Packet packet) {
// Encode latency, existence of targets, and number of targets.
packet.encode(latencyMillis);
multiTagResult.populatePacket(packet);
packet.encode((byte) targets.size());
// Encode the information of each target.
for (var target : targets) target.populatePacket(packet);
// Return the packet.
return packet;
}
@Override
public int hashCode() {
final int prime = 31;
@@ -236,4 +194,35 @@ public class PhotonPipelineResult {
+ multiTagResult
+ "]";
}
public static final class APacketSerde implements PacketSerde<PhotonPipelineResult> {
@Override
public int getMaxByteSize() {
// This uses dynamic packets so it doesn't matter
return -1;
}
@Override
public void pack(Packet packet, PhotonPipelineResult value) {
packet.encode(value.latencyMillis);
packet.encode((byte) value.targets.size());
for (var target : value.targets) PhotonTrackedTarget.serde.pack(packet, target);
MultiTargetPNPResult.serde.pack(packet, value.multiTagResult);
}
@Override
public PhotonPipelineResult unpack(Packet packet) {
var latency = packet.decodeDouble();
var len = packet.decodeByte();
var targets = new ArrayList<PhotonTrackedTarget>(len);
for (int i = 0; i < len; i++) {
targets.add(PhotonTrackedTarget.serde.unpack(packet));
}
var result = MultiTargetPNPResult.serde.unpack(packet);
return new PhotonPipelineResult(latency, targets, result);
}
}
public static final APacketSerde serde = new APacketSerde();
}

176
photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
View File

@@ -21,29 +21,26 @@ import edu.wpi.first.math.geometry.Transform3d;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
import org.photonvision.utils.PacketUtils;
public class PhotonTrackedTarget {
private static final int MAX_CORNERS = 8;
public static final int PACK_SIZE_BYTES =
Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS);
private double yaw;
private double pitch;
private double area;
private double skew;
private int fiducialId;
private Transform3d bestCameraToTarget = new Transform3d();
private Transform3d altCameraToTarget = new Transform3d();
private double poseAmbiguity;
private final double yaw;
private final double pitch;
private final double area;
private final double skew;
private final int fiducialId;
private final Transform3d bestCameraToTarget;
private final Transform3d altCameraToTarget;
private final double poseAmbiguity;
// Corners from the min-area rectangle bounding the target
private List<TargetCorner> minAreaRectCorners;
private final List<TargetCorner> minAreaRectCorners;
// Corners from whatever corner detection method was used
private List<TargetCorner> detectedCorners;
public PhotonTrackedTarget() {}
private final List<TargetCorner> detectedCorners;
/** Construct a tracked target, given exactly 4 corners */
public PhotonTrackedTarget(
@@ -197,81 +194,6 @@ public class PhotonTrackedTarget {
return true;
}
private static void encodeList(Packet packet, List<TargetCorner> list) {
packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE));
for (TargetCorner targetCorner : list) {
packet.encode(targetCorner.x);
packet.encode(targetCorner.y);
}
}
private static List<TargetCorner> decodeList(Packet p) {
byte len = p.decodeByte();
var ret = new ArrayList<TargetCorner>();
for (int i = 0; i < len; i++) {
double cx = p.decodeDouble();
double cy = p.decodeDouble();
ret.add(new TargetCorner(cx, cy));
}
return ret;
}
/**
* Populates the fields of this class with information from the incoming packet.
*
* @param packet The incoming packet.
* @return The incoming packet.
*/
public Packet createFromPacket(Packet packet) {
this.yaw = packet.decodeDouble();
this.pitch = packet.decodeDouble();
this.area = packet.decodeDouble();
this.skew = packet.decodeDouble();
this.fiducialId = packet.decodeInt();
this.bestCameraToTarget = PacketUtils.decodeTransform(packet);
this.altCameraToTarget = PacketUtils.decodeTransform(packet);
this.poseAmbiguity = packet.decodeDouble();
this.minAreaRectCorners = new ArrayList<>(4);
for (int i = 0; i < 4; i++) {
double cx = packet.decodeDouble();
double cy = packet.decodeDouble();
minAreaRectCorners.add(new TargetCorner(cx, cy));
}
detectedCorners = decodeList(packet);
return packet;
}
/**
* Populates the outgoing packet with information from the current target.
*
* @param packet The outgoing packet.
* @return The outgoing packet.
*/
public Packet populatePacket(Packet packet) {
packet.encode(yaw);
packet.encode(pitch);
packet.encode(area);
packet.encode(skew);
packet.encode(fiducialId);
PacketUtils.encodeTransform(packet, bestCameraToTarget);
PacketUtils.encodeTransform(packet, altCameraToTarget);
packet.encode(poseAmbiguity);
for (int i = 0; i < 4; i++) {
packet.encode(minAreaRectCorners.get(i).x);
packet.encode(minAreaRectCorners.get(i).y);
}
encodeList(packet, detectedCorners);
return packet;
}
@Override
public String toString() {
return "PhotonTrackedTarget{"
@@ -291,4 +213,80 @@ public class PhotonTrackedTarget {
+ minAreaRectCorners
+ '}';
}
public static final class APacketSerde implements PacketSerde<PhotonTrackedTarget> {
@Override
public int getMaxByteSize() {
return Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS);
}
@Override
public void pack(Packet packet, PhotonTrackedTarget value) {
packet.encode(value.yaw);
packet.encode(value.pitch);
packet.encode(value.area);
packet.encode(value.skew);
packet.encode(value.fiducialId);
if (value.fiducialId != -1) {
PacketUtils.packTransform3d(packet, value.bestCameraToTarget);
PacketUtils.packTransform3d(packet, value.altCameraToTarget);
packet.encode(value.poseAmbiguity);
}
for (int i = 0; i < 4; i++) {
TargetCorner.serde.pack(packet, value.minAreaRectCorners.get(i));
}
packet.encode((byte) Math.min(value.detectedCorners.size(), Byte.MAX_VALUE));
for (TargetCorner targetCorner : value.detectedCorners) {
TargetCorner.serde.pack(packet, targetCorner);
}
}
@Override
public PhotonTrackedTarget unpack(Packet packet) {
var yaw = packet.decodeDouble();
var pitch = packet.decodeDouble();
var area = packet.decodeDouble();
var skew = packet.decodeDouble();
var fiducialId = packet.decodeInt();
Transform3d best;
Transform3d alt;
double ambiguity;
if (fiducialId != -1.0) {
best = PacketUtils.unpackTransform3d(packet);
alt = PacketUtils.unpackTransform3d(packet);
ambiguity = packet.decodeDouble();
} else {
best = new Transform3d();
alt = new Transform3d();
ambiguity = -1.0;
}
var minAreaRectCorners = new ArrayList<TargetCorner>(4);
for (int i = 0; i < 4; i++) {
minAreaRectCorners.add(TargetCorner.serde.unpack(packet));
}
var len = packet.decodeByte();
var detectedCorners = new ArrayList<TargetCorner>(len);
for (int i = 0; i < len; i++) {
detectedCorners.add(TargetCorner.serde.unpack(packet));
}
return new PhotonTrackedTarget(
yaw,
pitch,
area,
skew,
fiducialId,
best,
alt,
ambiguity,
minAreaRectCorners,
detectedCorners);
}
}
public static final APacketSerde serde = new APacketSerde();
}

22
photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java
View File

@@ -18,6 +18,8 @@
package org.photonvision.targeting;
import java.util.Objects;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
/**
* Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels.
@@ -49,4 +51,24 @@ public class TargetCorner {
public String toString() {
return "(" + x + "," + y + ')';
}
public static final class APacketSerde implements PacketSerde<TargetCorner> {
@Override
public int getMaxByteSize() {
return Double.BYTES * 2;
}
@Override
public void pack(Packet packet, TargetCorner corner) {
packet.encode(corner.x);
packet.encode(corner.y);
}
@Override
public TargetCorner unpack(Packet packet) {
return new TargetCorner(packet.decodeDouble(), packet.decodeDouble());
}
}
public static final APacketSerde serde = new APacketSerde();
}

113
photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java
View File

@@ -17,33 +17,100 @@
package org.photonvision.utils;
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.geometry.*;
import org.photonvision.common.dataflow.structures.Packet;
public class PacketUtils {
public static Transform3d decodeTransform(Packet packet) {
double x = packet.decodeDouble();
double y = packet.decodeDouble();
double z = packet.decodeDouble();
var translation = new Translation3d(x, y, z);
double w = packet.decodeDouble();
x = packet.decodeDouble();
y = packet.decodeDouble();
z = packet.decodeDouble();
var rotation = new Rotation3d(new Quaternion(w, x, y, z));
return new Transform3d(translation, rotation);
public static final int ROTATION2D_BYTE_SIZE = Double.BYTES;
public static final int QUATERNION_BYTE_SIZE = Double.BYTES * 4;
public static final int ROTATION3D_BYTE_SIZE = QUATERNION_BYTE_SIZE;
public static final int TRANSLATION2D_BYTE_SIZE = Double.BYTES * 2;
public static final int TRANSLATION3D_BYTE_SIZE = Double.BYTES * 3;
public static final int TRANSFORM2D_BYTE_SIZE = TRANSLATION2D_BYTE_SIZE + ROTATION2D_BYTE_SIZE;
public static final int TRANSFORM3D_BYTE_SIZE = TRANSLATION3D_BYTE_SIZE + ROTATION3D_BYTE_SIZE;
public static final int POSE2D_BYTE_SIZE = TRANSLATION2D_BYTE_SIZE + ROTATION2D_BYTE_SIZE;
public static final int POSE3D_BYTE_SIZE = TRANSLATION3D_BYTE_SIZE + ROTATION3D_BYTE_SIZE;
public static void packRotation2d(Packet packet, Rotation2d rotation) {
packet.encode(rotation.getRadians());
}
public static void encodeTransform(Packet packet, Transform3d transform) {
packet.encode(transform.getTranslation().getX());
packet.encode(transform.getTranslation().getY());
packet.encode(transform.getTranslation().getZ());
packet.encode(transform.getRotation().getQuaternion().getW());
packet.encode(transform.getRotation().getQuaternion().getX());
packet.encode(transform.getRotation().getQuaternion().getY());
packet.encode(transform.getRotation().getQuaternion().getZ());
public static Rotation2d unpackRotation2d(Packet packet) {
return new Rotation2d(packet.decodeDouble());
}
public static void packQuaternion(Packet packet, Quaternion quaternion) {
packet.encode(quaternion.getW());
packet.encode(quaternion.getX());
packet.encode(quaternion.getY());
packet.encode(quaternion.getZ());
}
public static Quaternion unpackQuaternion(Packet packet) {
return new Quaternion(
packet.decodeDouble(), packet.decodeDouble(), packet.decodeDouble(), packet.decodeDouble());
}
public static void packRotation3d(Packet packet, Rotation3d rotation) {
packQuaternion(packet, rotation.getQuaternion());
}
public static Rotation3d unpackRotation3d(Packet packet) {
return new Rotation3d(unpackQuaternion(packet));
}
public static void packTranslation2d(Packet packet, Translation2d translation) {
packet.encode(translation.getX());
packet.encode(translation.getY());
}
public static Translation2d unpackTranslation2d(Packet packet) {
return new Translation2d(packet.decodeDouble(), packet.decodeDouble());
}
public static void packTranslation3d(Packet packet, Translation3d translation) {
packet.encode(translation.getX());
packet.encode(translation.getY());
packet.encode(translation.getZ());
}
public static Translation3d unpackTranslation3d(Packet packet) {
return new Translation3d(packet.decodeDouble(), packet.decodeDouble(), packet.decodeDouble());
}
public static void packTransform2d(Packet packet, Transform2d transform) {
packTranslation2d(packet, transform.getTranslation());
packRotation2d(packet, transform.getRotation());
}
public static Transform2d unpackTransform2d(Packet packet) {
return new Transform2d(unpackTranslation2d(packet), unpackRotation2d(packet));
}
public static void packTransform3d(Packet packet, Transform3d transform) {
packTranslation3d(packet, transform.getTranslation());
packRotation3d(packet, transform.getRotation());
}
public static Transform3d unpackTransform3d(Packet packet) {
return new Transform3d(unpackTranslation3d(packet), unpackRotation3d(packet));
}
public static void packPose2d(Packet packet, Pose2d pose) {
packTranslation2d(packet, pose.getTranslation());
packRotation2d(packet, pose.getRotation());
}
public static Pose2d unpackPose2d(Packet packet) {
return new Pose2d(unpackTranslation2d(packet), unpackRotation2d(packet));
}
public static void packPose3d(Packet packet, Pose3d pose) {
packTranslation3d(packet, pose.getTranslation());
packRotation3d(packet, pose.getRotation());
}
public static Pose3d unpackPose3d(Packet packet) {
return new Pose3d(unpackTranslation3d(packet), unpackRotation3d(packet));
}
}