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

Add Photonlib (#231)

Browse Source 
Merges Photonlib into Photonvision, along with the Photonlib code examples. Also creates a new PhotonTargeting library teams can depend on.
This commit is contained in:
Matt
2021-01-16 20:41:47 -08:00
committed by GitHub
parent 58b39f47aa
commit 2e1b3d0f83
79 changed files with 5867 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

@@ -20,13 +20,17 @@ package org.photonvision.common.dataflow.networktables;
import edu.wpi.first.networktables.EntryNotification;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import java.util.ArrayList;
import java.util.List;
import java.util.function.BooleanSupplier;
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.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.pipeline.result.SimplePipelineResult;
import org.photonvision.vision.target.TrackedTarget;
public class NTDataPublisher implements CVPipelineResultConsumer {
@@ -163,7 +167,9 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
@Override
public void accept(CVPipelineResult result) {
var simplified = new SimplePipelineResult(result);
var simplified =
new PhotonPipelineResult(
result.getLatencyMillis(), simpleFromTrackedTargets(result.targets));
Packet packet = new Packet(simplified.getPacketSize());
simplified.populatePacket(packet);
@@ -201,4 +207,14 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
}
rootTable.getInstance().flush();
}
public static List<PhotonTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
var ret = new ArrayList<PhotonTrackedTarget>();
for (var t : targets) {
ret.add(
new PhotonTrackedTarget(
t.getYaw(), t.getPitch(), t.getArea(), t.getSkew(), t.getCameraToTarget()));
}
return ret;
}
}

165
photon-core/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
View File

@@ -1,165 +0,0 @@
/*
* 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;
/** A packet that holds byte-packed data to be sent over NetworkTables. */
public class Packet {
// Size of the packet.
int size;
// Data stored in the packet.
byte[] packetData;
// Read and write positions.
int readPos, writePos;
/** Constructs an empty packet. */
public Packet(int size) {
this.size = size;
packetData = new byte[size];
}
/**
* Constructs a packet with the given data.
*
* @param data The packet data.
*/
public Packet(byte[] data) {
packetData = data;
size = packetData.length;
}
/** Clears the packet and resets the read and write positions. */
public void clear() {
packetData = new byte[size];
readPos = 0;
writePos = 0;
}
/**
* Returns the packet data.
*
* @return The packet data.
*/
public byte[] getData() {
return packetData;
}
/**
* Sets the packet data.
*
* @param data The packet data.
*/
public void setData(byte[] data) {
packetData = data;
size = data.length;
}
/**
* Encodes the byte into the packet.
*
* @param src The byte to encode.
*/
public void encode(byte src) {
packetData[writePos++] = src;
}
/**
* Encodes the integer into the packet.
*
* @param src The integer to encode.
*/
public void encode(int src) {
packetData[writePos++] = (byte) (src >>> 24);
packetData[writePos++] = (byte) (src >>> 16);
packetData[writePos++] = (byte) (src >>> 8);
packetData[writePos++] = (byte) src;
}
/**
* Encodes the double into the packet.
*
* @param src The double to encode.
*/
public void encode(double src) {
long data = Double.doubleToRawLongBits(src);
packetData[writePos++] = (byte) ((data >> 56) & 0xff);
packetData[writePos++] = (byte) ((data >> 48) & 0xff);
packetData[writePos++] = (byte) ((data >> 40) & 0xff);
packetData[writePos++] = (byte) ((data >> 32) & 0xff);
packetData[writePos++] = (byte) ((data >> 24) & 0xff);
packetData[writePos++] = (byte) ((data >> 16) & 0xff);
packetData[writePos++] = (byte) ((data >> 8) & 0xff);
packetData[writePos++] = (byte) (data & 0xff);
}
/**
* Encodes the boolean into the packet.
*
* @param src The boolean to encode.
*/
public void encode(boolean src) {
packetData[writePos++] = src ? (byte) 1 : (byte) 0;
}
/**
* Returns a decoded byte from the packet.
*
* @return A decoded byte from the packet.
*/
public byte decodeByte() {
return packetData[readPos++];
}
/**
* Returns a decoded int from the packet.
*
* @return A decoded int from the packet.
*/
public int decodeInt() {
return (0xff & packetData[readPos++]) << 24
| (0xff & packetData[readPos++]) << 16
| (0xff & packetData[readPos++]) << 8
| (0xff & packetData[readPos++]);
}
/**
* Returns a decoded double from the packet.
*
* @return A decoded double from the packet.
*/
public double decodeDouble() {
long data =
(long) (0xff & packetData[readPos++]) << 56
| (long) (0xff & packetData[readPos++]) << 48
| (long) (0xff & packetData[readPos++]) << 40
| (long) (0xff & packetData[readPos++]) << 32
| (long) (0xff & packetData[readPos++]) << 24
| (long) (0xff & packetData[readPos++]) << 16
| (long) (0xff & packetData[readPos++]) << 8
| (long) (0xff & packetData[readPos++]);
return Double.longBitsToDouble(data);
}
/**
* Returns a decoded boolean from the packet.
*
* @return A decoded boolean from the packet.
*/
public boolean decodeBoolean() {
return packetData[readPos++] == 1;
}
}

3
photon-core/src/main/java/org/photonvision/common/hardware/HardwareManager.java
View File

@@ -27,7 +27,6 @@ import org.photonvision.common.dataflow.networktables.NTDataChangeListener;
import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
import org.photonvision.common.hardware.GPIO.CustomGPIO;
import org.photonvision.common.hardware.GPIO.pi.PigpioSocket;
import org.photonvision.common.hardware.VisionLED.VisionLEDMode;
import org.photonvision.common.hardware.metrics.MetricsBase;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
@@ -91,7 +90,7 @@ public class HardwareManager {
pigpioSocket);
ledModeEntry = NetworkTablesManager.getInstance().kRootTable.getEntry("ledMode");
ledModeEntry.setNumber(VisionLEDMode.VLM_DEFAULT.value);
ledModeEntry.setNumber(VisionLEDMode.kDefault.value);
ledModeListener =
visionLED == null
? null

58
photon-core/src/main/java/org/photonvision/common/hardware/VisionLED.java
View File

@@ -40,7 +40,7 @@ public class VisionLED {
private final int brightnessMax;
private final PigpioSocket pigpioSocket;
private VisionLEDMode currentLedMode = VisionLEDMode.VLM_DEFAULT;
private VisionLEDMode currentLedMode = VisionLEDMode.kDefault;
private BooleanSupplier pipelineModeSupplier;
private int mappedBrightnessPercentage;
@@ -111,7 +111,7 @@ public class VisionLED {
}
public void setState(boolean on) {
setInternal(on ? VisionLEDMode.VLM_ON : VisionLEDMode.VLM_OFF, false);
setInternal(on ? VisionLEDMode.kOn : VisionLEDMode.kOff, false);
}
void onLedModeChange(EntryNotification entryNotification) {
@@ -120,20 +120,20 @@ public class VisionLED {
VisionLEDMode newLedMode;
switch (newLedModeRaw) {
case -1:
newLedMode = VisionLEDMode.VLM_DEFAULT;
newLedMode = VisionLEDMode.kDefault;
break;
case 0:
newLedMode = VisionLEDMode.VLM_OFF;
newLedMode = VisionLEDMode.kOff;
break;
case 1:
newLedMode = VisionLEDMode.VLM_ON;
newLedMode = VisionLEDMode.kOn;
break;
case 2:
newLedMode = VisionLEDMode.VLM_BLINK;
newLedMode = VisionLEDMode.kBlink;
break;
default:
logger.warn("User supplied invalid LED mode, falling back to Default");
newLedMode = VisionLEDMode.VLM_DEFAULT;
newLedMode = VisionLEDMode.kDefault;
break;
}
setInternal(newLedMode, true);
@@ -145,16 +145,16 @@ public class VisionLED {
if (fromNT) {
switch (newLedMode) {
case VLM_DEFAULT:
case kDefault:
setStateImpl(pipelineModeSupplier.getAsBoolean());
break;
case VLM_OFF:
case kOff:
setStateImpl(false);
break;
case VLM_ON:
case kOn:
setStateImpl(true);
break;
case VLM_BLINK:
case kBlink:
blinkImpl(85, -1);
break;
}
@@ -166,15 +166,15 @@ public class VisionLED {
+ newLedMode.toString()
+ "\"");
} else {
if (currentLedMode == VisionLEDMode.VLM_DEFAULT) {
if (currentLedMode == VisionLEDMode.kDefault) {
switch (newLedMode) {
case VLM_DEFAULT:
case kDefault:
setStateImpl(pipelineModeSupplier.getAsBoolean());
break;
case VLM_OFF:
case kOff:
setStateImpl(false);
break;
case VLM_ON:
case kOn:
setStateImpl(true);
break;
}
@@ -182,32 +182,4 @@ public class VisionLED {
logger.info("Changing LED internal state to " + newLedMode.toString());
}
}
public enum VisionLEDMode {
VLM_DEFAULT(-1),
VLM_OFF(0),
VLM_ON(1),
VLM_BLINK(2);
public final int value;
VisionLEDMode(int value) {
this.value = value;
}
@Override
public String toString() {
switch (this) {
case VLM_DEFAULT:
return "Default";
case VLM_OFF:
return "Off";
case VLM_ON:
return "On";
case VLM_BLINK:
return "Blink";
}
return "";
}
}
}

128
photon-core/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java
View File

@@ -1,128 +0,0 @@
/*
* 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.vision.pipeline.result;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.vision.target.TrackedTarget;
public class SimplePipelineResult {
private double latencyMillis;
private boolean hasTargets;
public final List<SimpleTrackedTarget> targets = new ArrayList<>();
public SimplePipelineResult() {}
public SimplePipelineResult(
double latencyMillis, boolean hasTargets, List<SimpleTrackedTarget> targets) {
this.latencyMillis = latencyMillis;
this.hasTargets = hasTargets;
this.targets.addAll(targets);
}
public SimplePipelineResult(CVPipelineResult r) {
this(r.processingMillis, r.hasTargets(), simpleFromTrackedTargets(r.targets));
}
/**
* Returns the size of the packet needed to store this pipeline result.
*
* @return The size of the packet needed to store this pipeline result.
*/
public int getPacketSize() {
return targets.size() * SimpleTrackedTarget.PACK_SIZE_BYTES + 8 + 2;
}
public double getLatencyMillis() {
return latencyMillis;
}
public boolean hasTargets() {
return hasTargets;
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
SimplePipelineResult that = (SimplePipelineResult) o;
boolean latencyMatch = Double.compare(that.latencyMillis, latencyMillis) == 0;
boolean hasTargetsMatch = that.hasTargets == hasTargets;
boolean targetsMatch = that.targets.equals(targets);
return latencyMatch && hasTargetsMatch && targetsMatch;
}
@Override
public int hashCode() {
return Objects.hash(latencyMillis, hasTargets, targets);
}
/**
* 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();
hasTargets = packet.decodeBoolean();
byte targetCount = packet.decodeByte();
targets.clear();
// Decode the information of each target.
for (int i = 0; i < (int) targetCount; ++i) {
var target = new SimpleTrackedTarget();
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);
packet.encode(hasTargets);
packet.encode((byte) targets.size());
// Encode the information of each target.
for (var target : targets) target.populatePacket(packet);
// Return the packet.
return packet;
}
private static List<SimpleTrackedTarget> simpleFromTrackedTargets(List<TrackedTarget> targets) {
var ret = new ArrayList<SimpleTrackedTarget>();
for (var t : targets) {
ret.add(new SimpleTrackedTarget(t));
}
return ret;
}
}

120
photon-core/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
View File

@@ -1,120 +0,0 @@
/*
* 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.vision.pipeline.result;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import java.util.Objects;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.vision.target.TrackedTarget;
public class SimpleTrackedTarget {
public static final int PACK_SIZE_BYTES = Double.BYTES * 7;
private double yaw;
private double pitch;
private double area;
private double skew;
private Transform2d cameraToTarget = new Transform2d();
public SimpleTrackedTarget() {}
public SimpleTrackedTarget(double yaw, double pitch, double area, double skew, Transform2d pose) {
this.yaw = yaw;
this.pitch = pitch;
this.area = area;
this.skew = skew;
cameraToTarget = pose;
}
public SimpleTrackedTarget(TrackedTarget t) {
this(t.getYaw(), t.getPitch(), t.getArea(), t.getSkew(), t.getCameraToTarget());
}
public double getYaw() {
return yaw;
}
public double getPitch() {
return pitch;
}
public double getArea() {
return area;
}
public Transform2d getCameraToTarget() {
return cameraToTarget;
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
SimpleTrackedTarget that = (SimpleTrackedTarget) o;
return Double.compare(that.yaw, yaw) == 0
&& Double.compare(that.pitch, pitch) == 0
&& Double.compare(that.area, area) == 0
&& Objects.equals(cameraToTarget, that.cameraToTarget);
}
@Override
public int hashCode() {
return Objects.hash(yaw, pitch, area, cameraToTarget);
}
/**
* 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) {
yaw = packet.decodeDouble();
pitch = packet.decodeDouble();
area = packet.decodeDouble();
skew = packet.decodeDouble();
double x = packet.decodeDouble();
double y = packet.decodeDouble();
double r = packet.decodeDouble();
cameraToTarget = new Transform2d(new Translation2d(x, y), Rotation2d.fromDegrees(r));
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(cameraToTarget.getTranslation().getX());
packet.encode(cameraToTarget.getTranslation().getY());
packet.encode(cameraToTarget.getRotation().getDegrees());
return packet;
}
}