mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
Add Photonlib (#231)
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
@@ -0,0 +1,169 @@
|
||||
/*
|
||||
* 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;
|
||||
}
|
||||
|
||||
public int getSize() {
|
||||
return size;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,46 @@
|
||||
/*
|
||||
* 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.hardware;
|
||||
|
||||
public enum VisionLEDMode {
|
||||
kDefault(-1),
|
||||
kOff(0),
|
||||
kOn(1),
|
||||
kBlink(2);
|
||||
|
||||
public final int value;
|
||||
|
||||
VisionLEDMode(int value) {
|
||||
this.value = value;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
switch (this) {
|
||||
case kDefault:
|
||||
return "Default";
|
||||
case kOff:
|
||||
return "Off";
|
||||
case kOn:
|
||||
return "On";
|
||||
case kBlink:
|
||||
return "Blink";
|
||||
}
|
||||
return "";
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,167 @@
|
||||
/*
|
||||
* 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.targeting;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Objects;
|
||||
import org.photonvision.common.dataflow.structures.Packet;
|
||||
|
||||
/** Represents a pipeline result from a PhotonCamera. */
|
||||
public class PhotonPipelineResult {
|
||||
|
||||
private static boolean HAS_WARNED = false;
|
||||
|
||||
// Targets to store.
|
||||
public final List<PhotonTrackedTarget> targets = new ArrayList<>();
|
||||
|
||||
// Latency in milliseconds.
|
||||
private double latencyMillis;
|
||||
|
||||
// Whether targets exist.
|
||||
private boolean hasTargets;
|
||||
|
||||
/** Constructs an empty pipeline result. */
|
||||
public PhotonPipelineResult() {}
|
||||
|
||||
/**
|
||||
* 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) {
|
||||
this.latencyMillis = latencyMillis;
|
||||
this.hasTargets = targets.size() != 0;
|
||||
this.targets.addAll(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() * PhotonTrackedTarget.PACK_SIZE_BYTES + 8 + 2;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the best target in this pipeline result. If there are no targets, this method will
|
||||
* return null. The best target is determined by the target sort mode in the PhotonVision UI.
|
||||
*
|
||||
* @return The best target of the pipeline result.
|
||||
*/
|
||||
public PhotonTrackedTarget getBestTarget() {
|
||||
if (!hasTargets && !HAS_WARNED) {
|
||||
String errStr =
|
||||
"This PhotonPipelineResult object has no targets associated with it! Please check hasTargets() "
|
||||
+ "before calling this method. For more information, please review the PhotonLib "
|
||||
+ "documentation at http://docs.photonvision.org";
|
||||
System.err.println(errStr);
|
||||
new Exception().printStackTrace();
|
||||
HAS_WARNED = true;
|
||||
}
|
||||
return hasTargets ? targets.get(0) : null;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the latency in the pipeline.
|
||||
*
|
||||
* @return The latency in the pipeline.
|
||||
*/
|
||||
public double getLatencyMillis() {
|
||||
return latencyMillis;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether the pipeline has targets.
|
||||
*
|
||||
* @return Whether the pipeline has targets.
|
||||
*/
|
||||
public boolean hasTargets() {
|
||||
return hasTargets;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a copy of the vector of targets.
|
||||
*
|
||||
* @return A copy of the vector of targets.
|
||||
*/
|
||||
public List<PhotonTrackedTarget> getTargets() {
|
||||
return new ArrayList<>(targets);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean equals(Object o) {
|
||||
if (this == o) return true;
|
||||
if (o == null || getClass() != o.getClass()) return false;
|
||||
PhotonPipelineResult that = (PhotonPipelineResult) 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 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);
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,115 @@
|
||||
/*
|
||||
* 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.targeting;
|
||||
|
||||
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;
|
||||
|
||||
public class PhotonTrackedTarget {
|
||||
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 PhotonTrackedTarget() {}
|
||||
|
||||
public PhotonTrackedTarget(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 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;
|
||||
PhotonTrackedTarget that = (PhotonTrackedTarget) 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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user