Skip to content

Commit

Permalink
More endiennessf fixes and such
Browse files Browse the repository at this point in the history
  • Loading branch information
@mcm001
mcm001 committed Sep 13, 2024
1 parent 1d231a2 commit 41425c3
Show file tree
Hide file tree
Showing 5 changed files with 40 additions and 52 deletions.
14 changes: 3 additions & 11 deletions ...on-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

package org.photonvision.common.dataflow.networktables;

import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEvent;
import edu.wpi.first.util.WPIUtilJNI;
Expand Down Expand Up @@ -159,16 +160,7 @@ public void accept(CVPipelineResult result) {
ts.targetSkewEntry.set(bestTarget.getSkew());

var pose = bestTarget.getBestCameraToTarget3d();
ts.targetPoseEntry.set(
new double[] {
pose.getTranslation().getX(),
pose.getTranslation().getY(),
pose.getTranslation().getZ(),
pose.getRotation().getQuaternion().getW(),
pose.getRotation().getQuaternion().getX(),
pose.getRotation().getQuaternion().getY(),
pose.getRotation().getQuaternion().getZ()
});
ts.targetPoseEntry.set(pose);

var targetOffsetPoint = bestTarget.getTargetOffsetPoint();
ts.bestTargetPosX.set(targetOffsetPoint.x);
Expand All @@ -178,7 +170,7 @@ public void accept(CVPipelineResult result) {
ts.targetYawEntry.set(0);
ts.targetAreaEntry.set(0);
ts.targetSkewEntry.set(0);
ts.targetPoseEntry.set(new double[] {0, 0, 0});
ts.targetPoseEntry.set(new Transform3d());
ts.bestTargetPosX.set(0);
ts.bestTargetPosY.set(0);
}
Expand Down
8 changes: 3 additions & 5 deletions photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.util.PixelFormat;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.RobotController;
Expand Down Expand Up @@ -588,7 +589,7 @@ public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimest
ts.targetPitchEntry.set(0.0, receiveTimestamp);
ts.targetYawEntry.set(0.0, receiveTimestamp);
ts.targetAreaEntry.set(0.0, receiveTimestamp);
ts.targetPoseEntry.set(new double[] {0.0, 0.0, 0.0}, receiveTimestamp);
ts.targetPoseEntry.set(new Transform3d(), receiveTimestamp);
ts.targetSkewEntry.set(0.0, receiveTimestamp);
} else {
var bestTarget = result.getBestTarget();
Expand All @@ -599,10 +600,7 @@ public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimest
ts.targetSkewEntry.set(bestTarget.getSkew(), receiveTimestamp);

var transform = bestTarget.getBestCameraToTarget();
double[] poseData = {
transform.getX(), transform.getY(), transform.getRotation().toRotation2d().getDegrees()
};
ts.targetPoseEntry.set(poseData, receiveTimestamp);
ts.targetPoseEntry.set(transform, receiveTimestamp);
}

ts.cameraIntrinsicsPublisher.set(prop.getIntrinsics().getData(), receiveTimestamp);
Expand Down
57 changes: 29 additions & 28 deletions photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -303,27 +303,27 @@ public int decodeInt() {
if (packetData.length < readPos + 3) {
return 0;
}
return (
(0xff & packetData[readPos++]
| (0xff & packetData[readPos++]) << 8
| (0xff & packetData[readPos++]) << 16
| 0xff & packetData[readPos++]) << 24
);
return ((0xff & packetData[readPos++]
| (0xff & packetData[readPos++]) << 8
| (0xff & packetData[readPos++]) << 16
| 0xff & packetData[readPos++])
<< 24);
}

public long decodeLong() {
if (packetData.length < (readPos + 7)) {
return 0;
}
long data =
(long) (0xff & packetData[readPos++]
| (long) (0xff & packetData[readPos++]) << 8
| (long) (0xff & packetData[readPos++]) << 16
| (long) (0xff & packetData[readPos++]) << 24
| (long) (0xff & packetData[readPos++]) << 32
| (long) (0xff & packetData[readPos++]) << 40
| (long) (0xff & packetData[readPos++]) << 48
| (long) (0xff & packetData[readPos++]) << 56);
(long)
(0xff & packetData[readPos++]
| (long) (0xff & packetData[readPos++]) << 8
| (long) (0xff & packetData[readPos++]) << 16
| (long) (0xff & packetData[readPos++]) << 24
| (long) (0xff & packetData[readPos++]) << 32
| (long) (0xff & packetData[readPos++]) << 40
| (long) (0xff & packetData[readPos++]) << 48
| (long) (0xff & packetData[readPos++]) << 56);
return data;
}

Expand All @@ -337,14 +337,15 @@ public double decodeDouble() {
return 0;
}
long data =
(long) (0xff & packetData[readPos++]
| (long) (0xff & packetData[readPos++]) << 8
| (long) (0xff & packetData[readPos++]) << 16
| (long) (0xff & packetData[readPos++]) << 24
| (long) (0xff & packetData[readPos++]) << 32
| (long) (0xff & packetData[readPos++]) << 40
| (long) (0xff & packetData[readPos++]) << 48
| (long) (0xff & packetData[readPos++]) << 56);
(long)
(0xff & packetData[readPos++]
| (long) (0xff & packetData[readPos++]) << 8
| (long) (0xff & packetData[readPos++]) << 16
| (long) (0xff & packetData[readPos++]) << 24
| (long) (0xff & packetData[readPos++]) << 32
| (long) (0xff & packetData[readPos++]) << 40
| (long) (0xff & packetData[readPos++]) << 48
| (long) (0xff & packetData[readPos++]) << 56);
return Double.longBitsToDouble(data);
}

Expand All @@ -358,12 +359,12 @@ public float decodeFloat() {
return 0;
}

int data = (
(0xff & packetData[readPos++]
| (0xff & packetData[readPos++]) << 8
| (0xff & packetData[readPos++]) << 16
| 0xff & packetData[readPos++]) << 24
);
int data =
((0xff & packetData[readPos++]
| (0xff & packetData[readPos++]) << 8
| (0xff & packetData[readPos++]) << 16
| 0xff & packetData[readPos++])
<< 24);
return Float.intBitsToFloat(data);
}

Expand Down
6 changes: 4 additions & 2 deletions photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

package org.photonvision.common.networktables;

import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.networktables.BooleanPublisher;
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.BooleanTopic;
Expand All @@ -28,6 +29,7 @@
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.ProtobufPublisher;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.StructPublisher;
import org.photonvision.targeting.PhotonPipelineResult;

/**
Expand Down Expand Up @@ -56,7 +58,7 @@ public class NTTopicSet {
public DoublePublisher targetPitchEntry;
public DoublePublisher targetYawEntry;
public DoublePublisher targetAreaEntry;
public DoubleArrayPublisher targetPoseEntry;
public StructPublisher<Transform3d> targetPoseEntry;
public DoublePublisher targetSkewEntry;

// The raw position of the best target, in pixels.
Expand Down Expand Up @@ -102,7 +104,7 @@ public void updateEntries() {
targetPitchEntry = subTable.getDoubleTopic("targetPitch").publish();
targetAreaEntry = subTable.getDoubleTopic("targetArea").publish();
targetYawEntry = subTable.getDoubleTopic("targetYaw").publish();
targetPoseEntry = subTable.getDoubleArrayTopic("targetPose").publish();
targetPoseEntry = subTable.getStructTopic("targetPose", Transform3d.struct).publish();
targetSkewEntry = subTable.getDoubleTopic("targetSkew").publish();

bestTargetPosX = subTable.getDoubleTopic("targetPixelsX").publish();
Expand Down
7 changes: 1 addition & 6 deletions photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,8 @@

import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;

import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.networktables.RawPublisher;

import java.util.Arrays;
import java.util.HashSet;
import java.util.Set;
import org.photonvision.common.dataflow.structures.Packet;
Expand All @@ -50,15 +46,14 @@ public PacketPublisher(RawPublisher publisher, PacketSerde<T> photonStruct) {
}
addSchemaImpl(photonStruct, new HashSet<>());

// TODO: don't hard-code this
this.publisher.getTopic().getInstance().addSchema(Transform3d.struct);
}

public void set(T value, int byteSize) {
var packet = new Packet(byteSize);
photonStruct.pack(packet, value);
// todo: trim to only the bytes we need to send
publisher.set(packet.getWrittenDataCopy());
System.out.println(Arrays.toString(packet.getWrittenDataCopy()));
}

public void set(T value) {
Expand Down

0 comments on commit 41425c3

Please sign in to comment.