8000 [WIP] Add entry listener to photon camera by mcm001 · Pull Request #504 · PhotonVision/photonvision · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

[WIP] Add entry listener to photon camera #504

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.

Sign up for GitHub

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

Closed
wants to merge 2 commits into from
Closed

[WIP] Add entry listener to photon camera #504

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5d2b7d2
Maybe?
mcm001 Oct 19, 2022
5ed31cf
Don't actually make get atomic
mcm001 Oct 19, 2022
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
56 changes: 41 additions & 15 deletions photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,18 @@

package org.photonvision;

import edu.wpi.first.networktables.EntryListenerFlags;
import edu.wpi.first.networktables.EntryNotification;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DriverStation;

import java.util.Optional;
import java.util.concurrent.ConcurrentLinkedQueue;
import java.util.concurrent.atomic.AtomicReference;
import java.util.concurrent.locks.ReentrantLock;

import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.hardware.VisionLEDMode;
import org.photonvision.targeting.PhotonPipelineResult;
Expand All @@ -47,6 +55,8 @@ public class PhotonCamera {

private static boolean VERSION_CHECK_ENABLED = true;

private final ConcurrentLinkedQueue<PhotonPipelineResult> m_queue = new ConcurrentLinkedQueue<>();

public static void setVersionCheckEnabled(boolean enabled) {
VERSION_CHECK_ENABLED = enabled;
}
Expand All @@ -72,6 +82,30 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
pipelineIndexEntry = rootTable.getEntry("pipelineIndex");
ledModeEntry = mainTable.getEntry("ledMode");
versionEntry = mainTable.getEntry("version");

rawBytesEntry.addListener(this::consumeRawBytesEntry, EntryListenerFlags.kUpdate);
}

private void consumeRawBytesEntry(EntryNotification notification) {
verifyVersion();

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

// Create latest result. FPGA timestamp is in units of wpi::Now() * 1e-6, so seconds, ideally
var ret = new PhotonPipelineResult(notification.getEntry().getLastChange() * 1e-6);

// Populate packet and create result.
var bytes = notification.value.getRaw();
packet.setData(bytes);
if (packet.getSize() > 1) {
ret.createFromPacket(packet);
}

synchronized (m_queue) {
m_queue.clear();
m_queue.add(ret);
}
}

/**
Expand All @@ -89,21 +123,13 @@ public PhotonCamera(String cameraName) {
* @return The latest pipeline result.
*/
public PhotonPipelineResult getLatestResult() {
verifyVersion();

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

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

// Populate packet and create result.
packet.setData(rawBytesEntry.getRaw(new byte[] {}));
if (packet.getSize() < 1) return ret;
ret.createFromPacket(packet);

// Return result.
return ret;
synchronized (m_queue) {
var ret = m_queue.peek();
if (ret == null) {
ret = new PhotonPipelineResult();
}
return ret;
}
}

/**
Expand Down
15 changes: 13 additions & 2 deletions photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,17 +32,24 @@ public class PhotonPipelineResult {
// Latency in milliseconds.
private double latencyMillis;

// Timestamp of reception on robot, in seconds
private double fpgaTimestamp;

/** Constructs an empty pipeline result. */
public PhotonPipelineResult() {}

/** Constructs an empty pipeline result. */
public PhotonPipelineResult(double fpgaTimestamp) { this.fpgaTimestamp = fpgaTimestamp; }

/**
* Constructs a pipeline result.
*
* @param latencyMillis The latency in the pipeline.
* @param targets The list of targets identified by the pipeline.
*/
public PhotonPipelineResult(double latencyMillis, List<PhotonTrackedTarget> targets) {
public PhotonPipelineResult(double fpgaTimestamp, double latencyMillis, List<PhotonTrackedTarget> targets) {
this.latencyMillis = latencyMillis;
this.fpgaTimestamp = fpgaTimestamp;
this.targets.addAll(targets);
}

Expand Down Expand Up @@ -92,6 +99,10 @@ public boolean hasTargets() {
return targets.size() > 0;
}

public double getFPGATimestamp() {
return fpgaTimestamp;
}

/**
* Returns a copy of the vector of targets.
*
Expand All @@ -113,7 +124,7 @@ public boolean equals(Object o) {

@Override
public int hashCode() {
return Objects.hash(latencyMillis, targets);
return Objects.hash(fpgaTimestamp, latencyMillis, targets);
}

/**
Expand Down
2 changes: 1 addition & 1 deletion photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@ public boolean equals(Object o) {

@Override
public int hashCode() {
return Objects.hash(yaw, pitch, area, cameraToTarget);
return Objects.hash(yaw, pitch, area, cameraToTarget, skew, fiducialId, poseAmbiguity, targetCorners);
}

/**
Expand Down
0