Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 8 commits into from
Dec 25, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@
import java.util.function.Consumer;
import java.util.function.Supplier;
import org.photonvision.common.dataflow.CVPipelineResultConsumer;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.networktables.NTTopicSet;
Expand Down Expand Up @@ -134,10 +133,8 @@ public void accept(CVPipelineResult result) {
result.getLatencyMillis(),
TrackedTarget.simpleFromTrackedTargets(result.targets),
result.multiTagResult);
Packet packet = new Packet(simplified.getPacketSize());
simplified.populatePacket(packet);

ts.rawBytesEntry.set(packet.getData());
ts.resultPublisher.accept(simplified, simplified.getPacketSize());

ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get());
ts.driverModePublisher.set(driverModeSupplier.getAsBoolean());
Expand Down
29 changes: 10 additions & 19 deletions photon-lib/src/main/java/org/photonvision/PhotonCamera.java
Original file line number Diff line number Diff line change
Expand Up @@ -42,14 +42,13 @@
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.RawSubscriber;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.Optional;
import java.util.Set;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.hardware.VisionLEDMode;
import org.photonvision.common.networktables.PacketSubscriber;
import org.photonvision.targeting.PhotonPipelineResult;

/** Represents a camera that is connected to PhotonVision. */
Expand All @@ -58,7 +57,7 @@ public class PhotonCamera implements AutoCloseable {
public static final String kTableName = "photonvision";

private final NetworkTable cameraTable;
RawSubscriber rawBytesEntry;
PacketSubscriber<PhotonPipelineResult> resultSubscriber;
BooleanPublisher driverModePublisher;
BooleanSubscriber driverModeSubscriber;
DoublePublisher latencyMillisEntry;
Expand All @@ -78,7 +77,7 @@ public class PhotonCamera implements AutoCloseable {

@Override
public void close() {
rawBytesEntry.close();
resultSubscriber.close();
driverModePublisher.close();
driverModeSubscriber.close();
latencyMillisEntry.close();
Expand Down Expand Up @@ -115,8 +114,6 @@ public static void setVersionCheckEnabled(boolean enabled) {
VERSION_CHECK_ENABLED = enabled;
}

Packet packet = new Packet(1);

/**
* Constructs a PhotonCamera from a root table.
*
Expand All @@ -130,11 +127,14 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
var photonvision_root_table = instance.getTable(kTableName);
this.cameraTable = photonvision_root_table.getSubTable(cameraName);
path = cameraTable.getPath();
rawBytesEntry =
var rawBytesEntry =
cameraTable
.getRawTopic("rawBytes")
.subscribe(
"rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
resultSubscriber =
new PacketSubscriber<>(
rawBytesEntry, PhotonPipelineResult.serde, new PhotonPipelineResult());
driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
Expand Down Expand Up @@ -177,21 +177,12 @@ public PhotonCamera(String cameraName) {
public PhotonPipelineResult getLatestResult() {
verifyVersion();

// Clear the packet.
packet.clear();

// Create latest result.
var ret = new PhotonPipelineResult();

// Populate packet and create result.
packet.setData(rawBytesEntry.get(new byte[] {}));

if (packet.getSize() < 1) return ret;
ret.createFromPacket(packet);
var ret = resultSubscriber.get();

// Set the timestamp of the result.
// getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
ret.setTimestampSeconds((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3);
ret.setTimestampSeconds(
(resultSubscriber.subscriber.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3);

// Return result.
return ret;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,6 @@
import org.opencv.imgproc.Imgproc;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.estimation.CameraTargetRelation;
import org.photonvision.estimation.OpenCVHelp;
Expand Down Expand Up @@ -564,9 +563,7 @@ public void submitProcessedFrame(PhotonPipelineResult result) {
public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimestamp) {
ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp);

var newPacket = new Packet(result.getPacketSize());
result.populatePacket(newPacket);
ts.rawBytesEntry.set(newPacket.getData(), receiveTimestamp);
ts.resultPublisher.accept(result, result.getPacketSize());

boolean hasTargets = result.hasTargets();
ts.hasTargetEntry.set(hasTargets, receiveTimestamp);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,6 @@
import java.util.List;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
Expand Down Expand Up @@ -143,9 +142,8 @@ public void submitProcessedFrame(

PhotonPipelineResult newResult =
new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResult());
var newPacket = new Packet(newResult.getPacketSize());
newResult.populatePacket(newPacket);
ts.rawBytesEntry.set(newPacket.getData());

ts.resultPublisher.accept(newResult, newResult.getPacketSize());

boolean hasTargets = newResult.hasTargets();
ts.hasTargetEntry.set(hasTargets);
Expand Down
190 changes: 0 additions & 190 deletions photon-lib/src/test/java/org/photonvision/PacketTest.java

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ public void testEmpty() {
if (packet.getSize() < 1) {
return;
}
ret.createFromPacket(packet);
PhotonPipelineResult.serde.pack(packet, ret);
});
}
}
Original file line number Diff line number Diff line change
@@ -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);
}
Loading