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

46
photon-lib/src/main/driver/cpp/VendorJNI.cpp Normal file
View File

@@ -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/>.
*/
#include "com_vendor_jni_VendorJNI.h"
#include "jni.h"
JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
// Check to ensure the JNI version is valid
JNIEnv* env;
if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
return JNI_ERR;
// In here is also where you store things like class references
// if they are ever needed
return JNI_VERSION_1_6;
}
JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {}
/*
* Class: com_vendor_jni_VendorJNI
* Method: initialize
* Signature: ()I
*/
JNIEXPORT jint JNICALL
Java_com_vendor_jni_VendorJNI_initialize
(JNIEnv*, jclass)
{
return 0;
}

22
photon-lib/src/main/driver/cpp/driversource.cpp Normal file
View File

@@ -0,0 +1,22 @@
/*
* 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/>.
*/
#include "driverheader.h"
extern "C" {
void c_doThing(void) {}
} // extern "C"

22
photon-lib/src/main/driver/include/driverheader.h Normal file
View File

@@ -0,0 +1,22 @@
/*
* 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/>.
*/
#pragma once
extern "C" {
void c_doThing(void);
} // extern "C"

4
photon-lib/src/main/driver/symbols.txt Normal file
View File

@@ -0,0 +1,4 @@
JNI_OnLoad
JNI_OnUnload
Java_com_vendor_jni_VendorJNI_initialize
c_doThing

198
photon-lib/src/main/java/org/photonvision/PhotonCamera.java Normal file
View File

@@ -0,0 +1,198 @@
/*
* 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;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.hardware.VisionLEDMode;
import org.photonvision.targeting.PhotonPipelineResult;
/** Represents a camera that is connected to PhotonVision. */
public class PhotonCamera {
final NetworkTableEntry rawBytesEntry;
final NetworkTableEntry driverModeEntry;
final NetworkTableEntry inputSaveImgEntry;
final NetworkTableEntry outputSaveImgEntry;
final NetworkTableEntry pipelineIndexEntry;
final NetworkTableEntry ledModeEntry;
final NetworkTable mainTable = NetworkTableInstance.getDefault().getTable("photonvision");
boolean driverMode;
int pipelineIndex;
VisionLEDMode mode;
Packet packet = new Packet(1);
/**
* Constructs a PhotonCamera from a root table.
*
* @param rootTable The root table that the camera is broadcasting information over.
*/
public PhotonCamera(NetworkTable rootTable) {
rawBytesEntry = rootTable.getEntry("rawBytes");
driverModeEntry = rootTable.getEntry("driverMode");
inputSaveImgEntry = rootTable.getEntry("inputSaveImgCmd");
outputSaveImgEntry = rootTable.getEntry("outputSaveImgCmd");
pipelineIndexEntry = rootTable.getEntry("pipelineIndex");
ledModeEntry = mainTable.getEntry("ledMode");
driverMode = driverModeEntry.getBoolean(false);
pipelineIndex = pipelineIndexEntry.getNumber(0).intValue();
getLEDMode();
}
/**
* Constructs a PhotonCamera from the name of the camera.
*
* @param cameraName The nickname of the camera (found in the PhotonVision UI).
*/
public PhotonCamera(String cameraName) {
this(NetworkTableInstance.getDefault().getTable("photonvision").getSubTable(cameraName));
}
/**
* Returns the latest pipeline result.
*
* @return The latest pipeline result.
*/
public PhotonPipelineResult getLatestResult() {
// 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;
}
/**
* Returns whether the camera is in driver mode.
*
* @return Whether the camera is in driver mode.
*/
public boolean getDriverMode() {
return driverMode;
}
/**
* Toggles driver mode.
*
* @param driverMode Whether to set driver mode.
*/
public void setDriverMode(boolean driverMode) {
if (this.driverMode != driverMode) {
this.driverMode = driverMode;
driverModeEntry.setBoolean(this.driverMode);
}
}
/**
* Request the camera to save a new image file from the input camera stream with overlays. Images
* take up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk
* space and eventually cause the system to stop working. Clear out images in
* /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
*/
public void takeInputSnapshot() {
inputSaveImgEntry.setBoolean(true);
}
/**
* Request the camera to save a new image file from the output stream with overlays. Images take
* up space in the filesystem of the PhotonCamera. Calling it frequently will fill up disk space
* and eventually cause the system to stop working. Clear out images in
* /opt/photonvision/photonvision_config/imgSaves frequently to prevent issues.
*/
public void takeOutputSnapshot() {
outputSaveImgEntry.setBoolean(true);
}
/**
* Returns the active pipeline index.
*
* @return The active pipeline index.
*/
public int getPipelineIndex() {
return pipelineIndex;
}
/**
* Allows the user to select the active pipeline index.
*
* @param index The active pipeline index.
*/
public void setPipelineIndex(int index) {
if (pipelineIndex != index) {
pipelineIndex = index;
pipelineIndexEntry.setNumber(pipelineIndex);
}
}
/**
* Returns the current LED mode.
*
* @return The current LED mode.
*/
public VisionLEDMode getLEDMode() {
int value = ledModeEntry.getNumber(-1).intValue();
switch (value) {
case 0:
mode = VisionLEDMode.kOff;
break;
case 1:
mode = VisionLEDMode.kOn;
break;
case 2:
mode = VisionLEDMode.kBlink;
break;
case -1:
default:
mode = VisionLEDMode.kDefault;
break;
}
return mode;
}
/**
* Sets the LED mode.
*
* @param led The mode to set to.
*/
public void setLED(VisionLEDMode led) {
if (led != mode) {
ledModeEntry.setNumber(led.value);
}
}
/**
* Returns whether the latest target result has targets.
*
* @return Whether the latest target result has targets.
*/
public boolean hasTargets() {
return getLatestResult().hasTargets();
}
}

166
photon-lib/src/main/java/org/photonvision/PhotonUtils.java Normal file
View File

@@ -0,0 +1,166 @@
/*
* 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;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
public final class PhotonUtils {
private PhotonUtils() {
// Utility class
}
/**
* Algorithm from https://docs.limelightvision.io/en/latest/cs_estimating_distance.html Estimates
* range to a target using the target's elevation. This method can produce more stable results
* than SolvePNP when well tuned, if the full 6d robot pose is not required. Note that this method
* requires the camera to have 0 roll (not be skewed clockwise or CCW relative to the floor), and
* for there to exist a height differential between goal and camera. The larger this differential,
* the more accurate the distance estimate will be.
*
* <p>Units can be converted using the {@link edu.wpi.first.wpilibj.util.Units} class.
*
* @param cameraHeightMeters The physical height of the camera off the floor in meters.
* @param targetHeightMeters The physical height of the target off the floor in meters. This
* should be the height of whatever is being targeted (i.e. if the targeting region is set to
* top, this should be the height of the top of the target).
* @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians.
* Positive values up.
* @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
* values up.
* @return The estimated distance to the target in meters.
*/
public static double calculateDistanceToTargetMeters(
double cameraHeightMeters,
double targetHeightMeters,
double cameraPitchRadians,
double targetPitchRadians) {
return (targetHeightMeters - cameraHeightMeters)
/ Math.tan(cameraPitchRadians + targetPitchRadians);
}
/**
* Estimate the {@link Translation2d} of the target relative to the camera.
*
* @param targetDistanceMeters The distance to the target in meters.
* @param yaw The observed yaw of the target.
* @return The target's camera-relative translation.
*/
public static Translation2d estimateCameraToTargetTranslation(
double targetDistanceMeters, Rotation2d yaw) {
return new Translation2d(
yaw.getCos() * targetDistanceMeters, yaw.getSin() * targetDistanceMeters);
}
/**
* Estimate the position of the robot in the field.
*
* @param cameraHeightMeters The physical height of the camera off the floor in meters.
* @param targetHeightMeters The physical height of the target off the floor in meters. This
* should be the height of whatever is being targeted (i.e. if the targeting region is set to
* top, this should be the height of the top of the target).
* @param cameraPitchRadians The pitch of the camera from the horizontal plane in radians.
* Positive values up.
* @param targetPitchRadians The pitch of the target in the camera's lens in radians. Positive
* values up.
* @param targetYaw The observed yaw of the target. Note that this *must* be CCW-positive, and
* Photon returns CW-positive.
* @param gyroAngle The current robot gyro angle, likely from odometry.
* @param fieldToTarget A Pose2d representing the target position in the field coordinate system.
* @param cameraToRobot The position of the robot relative to the camera. If the camera was
* mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be
* Transform2d(3 inches, 0 inches, 0 degrees).
* @return The position of the robot in the field.
*/
public static Pose2d estimateFieldToRobot(
double cameraHeightMeters,
double targetHeightMeters,
double cameraPitchRadians,
double targetPitchRadians,
Rotation2d targetYaw,
Rotation2d gyroAngle,
Pose2d fieldToTarget,
Transform2d cameraToRobot) {
return PhotonUtils.estimateFieldToRobot(
PhotonUtils.estimateCameraToTarget(
PhotonUtils.estimateCameraToTargetTranslation(
PhotonUtils.calculateDistanceToTargetMeters(
cameraHeightMeters, targetHeightMeters, cameraPitchRadians, targetPitchRadians),
targetYaw),
fieldToTarget,
gyroAngle),
fieldToTarget,
cameraToRobot);
}
/**
* Estimates a {@link Transform2d} that maps the camera position to the target position, using the
* robot's gyro. Note that the gyro angle provided *must* line up with the field coordinate system
* -- that is, it should read zero degrees when pointed towards the opposing alliance station, and
* increase as the robot rotates CCW.
*
* @param cameraToTargetTranslation A Translation2d that encodes the x/y position of the target
* relative to the camera.
* @param fieldToTarget A Pose2d representing the target position in the field coordinate system.
* @param gyroAngle The current robot gyro angle, likely from odometry.
* @return A Transform2d that takes us from the camera to the target.
*/
public static Transform2d estimateCameraToTarget(
Translation2d cameraToTargetTranslation, Pose2d fieldToTarget, Rotation2d gyroAngle) {
// This pose maps our camera at the origin out to our target, in the robot
// reference frame
// The translation part of this Transform2d is from the above step, and the
// rotation uses our robot's
// gyro.
return new Transform2d(
cameraToTargetTranslation, gyroAngle.times(-1).minus(fieldToTarget.getRotation()));
}
/**
* Estimates the pose of the robot in the field coordinate system, given the position of the
* target relative to the camera, the target relative to the field, and the robot relative to the
* camera.
*
* @param cameraToTarget The position of the target relative to the camera.
* @param fieldToTarget The position of the target in the field.
* @param cameraToRobot The position of the robot relative to the camera. If the camera was
* mounted 3 inches behind the "origin" (usually physical center) of the robot, this would be
* Transform2d(3 inches, 0 inches, 0 degrees).
* @return The position of the robot in the field.
*/
public static Pose2d estimateFieldToRobot(
Transform2d cameraToTarget, Pose2d fieldToTarget, Transform2d cameraToRobot) {
return estimateFieldToCamera(cameraToTarget, fieldToTarget).transformBy(cameraToRobot);
}
/**
* Estimates the pose of the camera in the field coordinate system, given the position of the
* target relative to the camera, and the target relative to the field. This *only* tracks the
* position of the camera, not the position of the robot itself.
*
* @param cameraToTarget The position of the target relative to the camera.
* @param fieldToTarget The position of the target in the field.
* @return The position of the camera in the field.
*/
public static Pose2d estimateFieldToCamera(Transform2d cameraToTarget, Pose2d fieldToTarget) {
var targetToCamera = cameraToTarget.inverse();
return fieldToTarget.transformBy(targetToCamera);
}
}

70
photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java Normal file
View File

@@ -0,0 +1,70 @@
/*
* 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;
import edu.wpi.first.networktables.NetworkTable;
import java.util.Arrays;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
public class SimPhotonCamera extends PhotonCamera {
/**
* Constructs a Simulated PhotonCamera from a root table.
*
* @param rootTable The root table that the camera is broadcasting information over.
*/
public SimPhotonCamera(NetworkTable rootTable) {
super(rootTable);
}
/**
* Constructs a Simulated PhotonCamera from the name of the camera.
*
* @param cameraName The nickname of the camera (found in the PhotonVision UI).
*/
public SimPhotonCamera(String cameraName) {
super(cameraName);
}
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latencyMillis
* @param targets Each target detected
*/
public void submitProcessedFrame(double latencyMillis, PhotonTrackedTarget... targets) {
submitProcessedFrame(latencyMillis, Arrays.asList(targets));
}
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latencyMillis
* @param tgtList List of targets detected
*/
public void submitProcessedFrame(double latencyMillis, List<PhotonTrackedTarget> tgtList) {
if (!getDriverMode()) {
PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, tgtList);
var newPacket = new Packet(newResult.getPacketSize());
newResult.populatePacket(newPacket);
rawBytesEntry.setRaw(newPacket.getData());
}
}
}

181
photon-lib/src/main/java/org/photonvision/SimVisionSystem.java Normal file
View File

@@ -0,0 +1,181 @@
/*
* 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;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.util.Units;
import java.util.ArrayList;
import org.photonvision.targeting.PhotonTrackedTarget;
public class SimVisionSystem {
SimPhotonCamera cam;
double camDiagFOVDegrees;
double camHorizFOVDegrees;
double camVertFOVDegrees;
double cameraHeightOffGroundMeters;
double maxLEDRangeMeters;
double camPitchDegrees;
int cameraResWidth;
int cameraResHeight;
double minTargetArea;
Transform2d cameraToRobot;
ArrayList<SimVisionTarget> tgtList;
/**
* Create a simulated vision system involving a camera and coprocessor mounted on a mobile robot
* running Photonvision, detecting one or more targets scattered around the field. This assumes a
* fairly simple and distortionless pinhole camera model.
*
* @param camName Name of the photonvision camera to create. Align it with the settings you use in
* the PhotonVision GUI.
* @param camDiagFOVDegrees Diagonal Field of View of the camera used. Align it with the
* manufacturer specifications, and/or whatever is configured in the PhotonVision Setting
* page.
* @param camPitchDegrees pitch of the camera's view axis back from horizontal. Make this the same
* as whatever is configured in the PhotonVision Setting page.
* @param cameraToRobot Pose Transform to move from the camera's mount position to the robot's
* position
* @param cameraHeightOffGroundMeters Height of the camera off the ground in meters
* @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and
* make it visible. Set to 9000 or more if your vision system does not rely on LED's.
* @param cameraResWidth Width of your camera's image sensor in pixels
* @param cameraResHeight Height of your camera's image sensor in pixels
* @param minTargetArea Minimum area that that the target should be before it's recognized as a
* target by the camera. Match this with your contour filtering settings in the PhotonVision
* GUI.
*/
public SimVisionSystem(
String camName,
double camDiagFOVDegrees,
double camPitchDegrees,
Transform2d cameraToRobot,
double cameraHeightOffGroundMeters,
double maxLEDRangeMeters,
int cameraResWidth,
int cameraResHeight,
double minTargetArea) {
this.camDiagFOVDegrees = camDiagFOVDegrees;
this.camPitchDegrees = camPitchDegrees;
this.cameraToRobot = cameraToRobot;
this.cameraHeightOffGroundMeters = cameraHeightOffGroundMeters;
this.maxLEDRangeMeters = maxLEDRangeMeters;
this.cameraResWidth = cameraResWidth;
this.cameraResHeight = cameraResHeight;
this.minTargetArea = minTargetArea;
// Calculate horizontal/vertical FOV by similar triangles
double hypotPixels = Math.hypot(cameraResWidth, cameraResHeight);
this.camHorizFOVDegrees = camDiagFOVDegrees * cameraResWidth / hypotPixels;
this.camVertFOVDegrees = camDiagFOVDegrees * cameraResHeight / hypotPixels;
cam = new SimPhotonCamera(camName);
tgtList = new ArrayList<SimVisionTarget>();
}
/**
* Add a target on the field which your vision system is designed to detect. The photoncamera from
* this system will report the location of the robot relative to the subste of these targets which
* are visible from the given robot position.
*
* @param tgt
*/
public void addSimVisionTarget(SimVisionTarget tgt) {
tgtList.add(tgt);
}
/**
* Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or
* turret or some other mobile platform.
*
* @param newCameraToRobot New Tranform from the robot to the camera
* @param newCamHeightMeters New height of the camera off the floor
* @param newCamPitchDegrees New pitch of the camera axis back from horizontal
*/
public void moveCamera(
Transform2d newCameraToRobot, double newCamHeightMeters, double newCamPitchDegrees) {
this.cameraToRobot = newCameraToRobot;
this.cameraHeightOffGroundMeters = newCamHeightMeters;
this.camPitchDegrees = newCamPitchDegrees;
}
/**
* Periodic update. Call this once per frame of image data you wish to process and send to
* NetworkTables
*
* @param robotPoseMeters current pose of the robot on the field. Will be used to calcualte which
* targets are actually in view, where they are at relative to the robot, and relevant
* PhotonVision parameters.
*/
public void processFrame(Pose2d robotPoseMeters) {
Pose2d cameraPos = robotPoseMeters.transformBy(cameraToRobot.inverse());
ArrayList<PhotonTrackedTarget> visibleTgtList = new ArrayList<>(tgtList.size());
tgtList.forEach(
(tgt) -> {
var camToTargetTrans = new Transform2d(cameraPos, tgt.targetPos);
double distAlongGroundMeters = camToTargetTrans.getTranslation().getNorm();
double distVerticalMeters =
tgt.targetHeightAboveGroundMeters - this.cameraHeightOffGroundMeters;
double distMeters = Math.hypot(distAlongGroundMeters, distVerticalMeters);
double area = tgt.tgtAreaMeters2 / getM2PerPx(distAlongGroundMeters);
// 2D yaw mode considers the target as a point, and should ignore target rotation.
// Photon reports it in the correct robot reference frame.
// IE: targets to the left of the image should report negative yaw.
double yawDegrees =
-1.0
* Units.radiansToDegrees(
Math.atan2(
camToTargetTrans.getTranslation().getY(),
camToTargetTrans.getTranslation().getX()));
double pitchDegrees =
Units.radiansToDegrees(Math.atan2(distVerticalMeters, distAlongGroundMeters))
- this.camPitchDegrees;
if (camCanSeeTarget(distMeters, yawDegrees, pitchDegrees, area)) {
visibleTgtList.add(
new PhotonTrackedTarget(yawDegrees, pitchDegrees, area, 0.0, camToTargetTrans));
}
});
cam.submitProcessedFrame(0.0, visibleTgtList);
}
double getM2PerPx(double dist) {
double widthMPerPx =
2 * dist * Math.tan(Units.degreesToRadians(this.camHorizFOVDegrees) / 2) / cameraResWidth;
double heightMPerPx =
2 * dist * Math.tan(Units.degreesToRadians(this.camVertFOVDegrees) / 2) / cameraResHeight;
return widthMPerPx * heightMPerPx;
}
boolean camCanSeeTarget(double distMeters, double yaw, double pitch, double area) {
boolean inRange = (distMeters < this.maxLEDRangeMeters);
boolean inHorizAngle = Math.abs(yaw) < (this.camHorizFOVDegrees / 2);
boolean inVertAngle = Math.abs(pitch) < (this.camVertFOVDegrees / 2);
boolean targetBigEnough = area > this.minTargetArea;
return (inRange && inHorizAngle && inVertAngle && targetBigEnough);
}
}

51
photon-lib/src/main/java/org/photonvision/SimVisionTarget.java Normal file
View File

@@ -0,0 +1,51 @@
/*
* 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;
import edu.wpi.first.wpilibj.geometry.Pose2d;
public class SimVisionTarget {
Pose2d targetPos;
double targetWidthMeters;
double targetHeightMeters;
double targetHeightAboveGroundMeters;
double targetInfill_pct;
double tgtAreaMeters2;
/**
* Describes a vision target located somewhere on the field that your SimVisionSystem can detect.
*
* @param targetPos Pose2d of the target on the field. Define it such that, if you are standing on
* the middle of the field facing the target, the Y axis points to your left, and the X axis
* points away from you.
* @param targetHeightAboveGroundMeters Height of the target above the field plane, in meters.
* @param targetWidthMeters Width of the outter bounding box of the target in meters.
* @param targetHeightMeters Pair Height of the outter bounding box of the target in meters.
*/
public SimVisionTarget(
Pose2d targetPos,
double targetHeightAboveGroundMeters,
double targetWidthMeters,
double targetHeightMeters) {
this.targetPos = targetPos;
this.targetHeightAboveGroundMeters = targetHeightAboveGroundMeters;
this.targetWidthMeters = targetWidthMeters;
this.targetHeightMeters = targetHeightMeters;
this.tgtAreaMeters2 = targetWidthMeters * targetHeightMeters;
}
}

90
photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp Normal file
View File

@@ -0,0 +1,90 @@
/*
* 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/>.
*/
#include "photonlib/PhotonCamera.h"
#include "photonlib/Packet.h"
namespace photonlib {
PhotonCamera::PhotonCamera(std::shared_ptr<nt::NetworkTable> rootTable)
: rawBytesEntry(rootTable->GetEntry("rawBytes")),
driverModeEntry(rootTable->GetEntry("driverMode")),
inputSaveImgEntry(rootTable->GetEntry("inputSaveImgCmd")),
outputSaveImgEntry(rootTable->GetEntry("outputSaveImgCmd")),
pipelineIndexEntry(rootTable->GetEntry("pipelineIndex")),
ledModeEntry(mainTable->GetEntry("ledMode")) {
driverMode = driverModeEntry.GetBoolean(false);
pipelineIndex = static_cast<int>(pipelineIndexEntry.GetDouble(0.0));
mode = GetLEDMode();
}
PhotonCamera::PhotonCamera(const std::string& cameraName)
: PhotonCamera(nt::NetworkTableInstance::GetDefault()
.GetTable("photonvision")
->GetSubTable(cameraName)) {}
PhotonPipelineResult PhotonCamera::GetLatestResult() const {
// Clear the current packet.
packet.Clear();
// Create the new result;
PhotonPipelineResult result;
// Fill the packet with latest data and populate result.
std::string value = rawBytesEntry.GetValue()->GetRaw();
std::vector<char> bytes{value.begin(), value.end()};
photonlib::Packet packet{bytes};
packet >> result;
return result;
}
void PhotonCamera::SetDriverMode(bool driverMode) {
if (this->driverMode != driverMode) {
this->driverMode = driverMode;
driverModeEntry.SetBoolean(this->driverMode);
}
}
void PhotonCamera::TakeInputSnapshot() { inputSaveImgEntry.SetBoolean(true); }
void PhotonCamera::TakeOutputSnapshot() { outputSaveImgEntry.SetBoolean(true); }
bool PhotonCamera::GetDriverMode() const { return driverMode; }
void PhotonCamera::SetPipelineIndex(int index) {
if (index != pipelineIndex) {
pipelineIndex = index;
pipelineIndexEntry.SetDouble(static_cast<double>(pipelineIndex));
}
}
int PhotonCamera::GetPipelineIndex() const { return pipelineIndex; }
LEDMode PhotonCamera::GetLEDMode() const {
mode = static_cast<LEDMode>(static_cast<int>(ledModeEntry.GetDouble(-1.0)));
return mode;
}
void PhotonCamera::SetLEDMode(LEDMode led) {
if (led != mode) {
mode = led;
ledModeEntry.SetDouble(static_cast<double>(static_cast<int>(mode)));
}
}
} // namespace photonlib

67
photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp Normal file
View File

@@ -0,0 +1,67 @@
/*
* 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/>.
*/
#include "photonlib/PhotonPipelineResult.h"
namespace photonlib {
PhotonPipelineResult::PhotonPipelineResult(
units::second_t latency, wpi::ArrayRef<PhotonTrackedTarget> targets)
: latency(latency),
targets(targets.data(), targets.data() + targets.size()) {
hasTargets = targets.size() != 0;
}
bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const {
return latency == other.latency && hasTargets == other.hasTargets &&
targets == other.targets;
}
bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const {
return !operator==(other);
}
Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
// Encode latency, existence of targets, and number of targets.
packet << result.latency.to<double>() * 1000 << result.hasTargets
<< static_cast<int8_t>(result.targets.size());
// Encode the information of each target.
for (auto& target : result.targets) packet << target;
// Return the packet
return packet;
}
Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
// Decode latency, existence of targets, and number of targets.
int8_t targetCount = 0;
double latencyMillis = 0;
packet >> latencyMillis >> result.hasTargets >> targetCount;
result.latency = units::second_t(latencyMillis / 1000.0);
result.targets.clear();
// Decode the information of each target.
for (int i = 0; i < targetCount; ++i) {
PhotonTrackedTarget target;
packet >> target;
result.targets.push_back(target);
}
return packet;
}
} // namespace photonlib

60
photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp Normal file
View File

@@ -0,0 +1,60 @@
/*
* 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/>.
*/
#include "photonlib/PhotonTrackedTarget.h"
#include <iostream>
#include <frc/geometry/Translation2d.h>
namespace photonlib {
PhotonTrackedTarget::PhotonTrackedTarget(double yaw, double pitch, double area,
double skew,
const frc::Transform2d& pose)
: yaw(yaw), pitch(pitch), area(area), skew(skew), cameraToTarget(pose) {}
bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
return other.yaw == yaw && other.pitch == pitch && other.area == area &&
other.skew == skew && other.cameraToTarget == cameraToTarget;
}
bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
return !operator==(other);
}
Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
return packet << target.yaw << target.pitch << target.area << target.skew
<< target.cameraToTarget.Translation().X().to<double>()
<< target.cameraToTarget.Translation().Y().to<double>()
<< target.cameraToTarget.Rotation().Degrees().to<double>();
}
Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
packet >> target.yaw >> target.pitch >> target.area >> target.skew;
double x = 0;
double y = 0;
double rot = 0;
packet >> x >> y >> rot;
target.cameraToTarget =
frc::Transform2d(frc::Translation2d(units::meter_t(x), units::meter_t(y)),
units::degree_t(rot));
return packet;
}
} // namespace photonlib

42
photon-lib/src/main/native/cpp/photonlib/SimPhotonCamera.cpp Normal file
View File

@@ -0,0 +1,42 @@
/*
* 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/>.
*/
#include "photonlib/SimPhotonCamera.h"
namespace photonlib {
SimPhotonCamera::SimPhotonCamera(std::shared_ptr<nt::NetworkTable> rootTable)
: PhotonCamera(rootTable) {}
SimPhotonCamera::SimPhotonCamera(const std::string& cameraName)
: PhotonCamera(cameraName) {}
void SimPhotonCamera::SubmitProcessedFrame(
units::second_t latency, wpi::ArrayRef<PhotonTrackedTarget> tgtList) {
if (!GetDriverMode()) {
// Clear the current packet.
simPacket.Clear();
// Create the new result and pump it into the packet
simPacket << PhotonPipelineResult(latency, tgtList);
rawBytesEntry.SetRaw(
wpi::StringRef(simPacket.GetData().data(), simPacket.GetData().size()));
}
}
} // namespace photonlib

119
photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp Normal file
View File

@@ -0,0 +1,119 @@
/*
* 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/>.
*/
#include "photonlib/SimVisionSystem.h"
#include <cmath>
#include <units/angle.h>
#include <units/length.h>
namespace photonlib {
SimVisionSystem::SimVisionSystem(const std::string& name,
units::degree_t camDiagFOV,
units::degree_t camPitch,
frc::Transform2d cameraToRobot,
units::meter_t cameraHeightOffGround,
units::meter_t maxLEDRange, int cameraResWidth,
int cameraResHeight, double minTargetArea)
: camDiagFOV(camDiagFOV),
camPitch(camPitch),
cameraToRobot(cameraToRobot),
cameraHeightOffGround(cameraHeightOffGround),
maxLEDRange(maxLEDRange),
cameraResWidth(cameraResWidth),
cameraResHeight(cameraResHeight),
minTargetArea(minTargetArea) {
double hypotPixels = std::hypot(cameraResWidth, cameraResHeight);
camHorizFOV = camDiagFOV * cameraResWidth / hypotPixels;
camVertFOV = camDiagFOV * cameraResHeight / hypotPixels;
cam = SimPhotonCamera(name);
tgtList.clear();
}
void SimVisionSystem::AddSimVisionTarget(SimVisionTarget tgt) {
tgtList.push_back(tgt);
}
void SimVisionSystem::MoveCamera(frc::Transform2d newCameraToRobot,
units::meter_t newCamHeight,
units::degree_t newCamPitch) {
cameraToRobot = newCameraToRobot;
cameraHeightOffGround = newCamHeight;
camPitch = newCamPitch;
}
void SimVisionSystem::ProcessFrame(frc::Pose2d robotPose) {
frc::Pose2d cameraPos = robotPose.TransformBy(cameraToRobot.Inverse());
std::vector<PhotonTrackedTarget> visibleTgtList = {};
for (auto&& tgt : tgtList) {
frc::Transform2d camToTargetTrans =
frc::Transform2d(cameraPos, tgt.targetPos);
units::meter_t distAlongGround = camToTargetTrans.Translation().Norm();
units::meter_t distVertical =
tgt.targetHeightAboveGround - cameraHeightOffGround;
units::meter_t distHypot =
units::math::hypot(distAlongGround, distVertical);
double area = tgt.tgtArea.to<double>() / GetM2PerPx(distAlongGround);
// 2D yaw mode considers the target as a point, and should ignore target
// rotation.
// Photon reports it in the correct robot reference frame.
// IE: targets to the left of the image should report negative yaw.
units::degree_t yawAngle =
-1.0 * units::math::atan2(camToTargetTrans.Translation().Y(),
camToTargetTrans.Translation().X());
units::degree_t pitchAngle =
units::math::atan2(distVertical, distAlongGround) - camPitch;
if (CamCanSeeTarget(distHypot, yawAngle, pitchAngle, area)) {
PhotonTrackedTarget newTgt =
PhotonTrackedTarget(yawAngle.to<double>(), pitchAngle.to<double>(),
area, 0.0, camToTargetTrans);
visibleTgtList.push_back(newTgt);
}
}
units::second_t procDelay(0.0); // Future - tie this to something meaningful
cam.SubmitProcessedFrame(
procDelay, wpi::MutableArrayRef<PhotonTrackedTarget>(visibleTgtList));
}
double SimVisionSystem::GetM2PerPx(units::meter_t dist) {
double heightMPerPx = 2 * dist.to<double>() *
units::math::tan(camVertFOV / 2) / cameraResHeight;
double widthMPerPx = 2 * dist.to<double>() *
units::math::tan(camHorizFOV / 2) / cameraResWidth;
return widthMPerPx * heightMPerPx;
}
bool SimVisionSystem::CamCanSeeTarget(units::meter_t distHypot,
units::degree_t yaw,
units::degree_t pitch, double area) {
bool inRange = (distHypot < maxLEDRange);
bool inHorizAngle = units::math::abs(yaw) < (camHorizFOV / 2);
bool inVertAngle = units::math::abs(pitch) < (camVertFOV / 2);
bool targetBigEnough = area > minTargetArea;
return (inRange && inHorizAngle && inVertAngle && targetBigEnough);
}
} // namespace photonlib

33
photon-lib/src/main/native/cpp/photonlib/SimVisionTarget.cpp Normal file
View File

@@ -0,0 +1,33 @@
/*
* 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/>.
*/
#include "photonlib/SimVisionTarget.h"
namespace photonlib {
SimVisionTarget::SimVisionTarget(frc::Pose2d& targetPos,
units::meter_t targetHeightAboveGround,
units::meter_t targetWidth,
units::meter_t targetHeight)
: targetPos(targetPos),
targetHeightAboveGround(targetHeightAboveGround),
targetWidth(targetWidth),
targetHeight(targetHeight) {
tgtArea = targetWidth * targetHeight;
}
} // namespace photonlib

121
photon-lib/src/main/native/include/photonlib/Packet.h Normal file
View File

@@ -0,0 +1,121 @@
/*
* 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/>.
*/
#pragma once
#include <algorithm>
#include <string>
#include <vector>
#include <wpi/Endian.h>
namespace photonlib {
/**
* A packet that holds byte-packed data to be sent over NetworkTables.
*/
class Packet {
public:
/**
* Constructs an empty packet.
*/
Packet() = default;
/**
* Constructs a packet with the given data.
* @param data The packet data.
*/
explicit Packet(std::vector<char> data) : packetData(data) {}
/**
* Clears the packet and resets the read and write positions.
*/
void Clear() {
packetData.clear();
readPos = 0;
writePos = 0;
}
/**
* Returns the packet data.
* @return The packet data.
*/
const std::vector<char>& GetData() { return packetData; }
/**
* Returns the number of bytes in the data.
* @return The number of bytes in the data.
*/
size_t GetDataSize() const { return packetData.size(); }
/**
* Adds a value to the data buffer. This should only be used with PODs.
* @tparam T The data type.
* @param src The data source.
* @return A reference to the current object.
*/
template <typename T>
Packet& operator<<(T src) {
packetData.resize(packetData.size() + sizeof(T));
std::memcpy(packetData.data() + writePos, &src, sizeof(T));
if constexpr (wpi::support::endian::system_endianness() ==
wpi::support::endianness::little) {
// Reverse to big endian for network conventions.
std::reverse(packetData.data() + writePos,
packetData.data() + writePos + sizeof(T));
}
writePos += sizeof(T);
return *this;
}
/**
* Extracts a value to the provided destination.
* @tparam T The type of value to extract.
* @param value The value to extract.
* @return A reference to the current object.
*/
template <typename T>
Packet& operator>>(T& value) {
std::memcpy(&value, packetData.data() + readPos, sizeof(T));
if constexpr (wpi::support::endian::system_endianness() ==
wpi::support::endianness::little) {
// Reverse to little endian for host.
char& raw = reinterpret_cast<char&>(value);
std::reverse(&raw, &raw + sizeof(T));
}
readPos += sizeof(T);
return *this;
}
bool operator==(const Packet& right) const {
return packetData == right.packetData;
}
bool operator!=(const Packet& right) const { return !operator==(right); }
private:
// Data stored in the packet
std::vector<char> packetData;
size_t readPos = 0;
size_t writePos = 0;
};
} // namespace photonlib

141
photon-lib/src/main/native/include/photonlib/PhotonCamera.h Normal file
View File

@@ -0,0 +1,141 @@
/*
* 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/>.
*/
#pragma once
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableInstance.h>
#include <memory>
#include <string>
#include "photonlib/PhotonPipelineResult.h"
namespace photonlib {
enum LEDMode : int { kDefault = -1, kOff = 0, kOn = 1, kBlink = 2 };
/**
* Represents a camera that is connected to PhotonVision.ß
*/
class PhotonCamera {
public:
/**
* Constructs a PhotonCamera from a root table.
* @param rootTable The root table that the camera is broadcasting information
* over.
*/
explicit PhotonCamera(std::shared_ptr<nt::NetworkTable> rootTable);
/**
* Constructs a PhotonCamera from the name of the camera.
* @param cameraName The nickname of the camera (found in the PhotonVision
* UI).
*/
explicit PhotonCamera(const std::string& cameraName);
/**
* Returns the latest pipeline result.
* @return The latest pipeline result.
*/
PhotonPipelineResult GetLatestResult() const;
/**
* Toggles driver mode.
* @param driverMode Whether to set driver mode.
*/
void SetDriverMode(bool driverMode);
/**
* Returns whether the camera is in driver mode.
* @return Whether the camera is in driver mode.
*/
bool GetDriverMode() const;
/**
* Request the camera to save a new image file from the input
* camera stream with overlays.
* Images take up space in the filesystem of the PhotonCamera.
* Calling it frequently will fill up disk space and eventually
* cause the system to stop working.
* Clear out images in /opt/photonvision/photonvision_config/imgSaves
* frequently to prevent issues.
*/
void TakeInputSnapshot(void);
/**
* Request the camera to save a new image file from the output
* stream with overlays.
* Images take up space in the filesystem of the PhotonCamera.
* Calling it frequently will fill up disk space and eventually
* cause the system to stop working.
* Clear out images in /opt/photonvision/photonvision_config/imgSaves
* frequently to prevent issues.
*/
void TakeOutputSnapshot(void);
/**
* Allows the user to select the active pipeline index.
* @param index The active pipeline index.
*/
void SetPipelineIndex(int index);
/**
* Returns the active pipeline index.
* @return The active pipeline index.
*/
int GetPipelineIndex() const;
/**
* Returns the current LED mode.
* @return The current LED mode.
*/
LEDMode GetLEDMode() const;
/**
* Sets the LED mode.
* @param led The mode to set to.
*/
void SetLEDMode(LEDMode led);
/**
* Returns whether the latest target result has targets.
* @return Whether the latest target result has targets.
*/
bool HasTargets() const { return GetLatestResult().HasTargets(); }
private:
std::shared_ptr<nt::NetworkTable> mainTable =
nt::NetworkTableInstance::GetDefault().GetTable("photonvision");
protected:
nt::NetworkTableEntry rawBytesEntry;
nt::NetworkTableEntry driverModeEntry;
nt::NetworkTableEntry inputSaveImgEntry;
nt::NetworkTableEntry outputSaveImgEntry;
nt::NetworkTableEntry pipelineIndexEntry;
nt::NetworkTableEntry ledModeEntry;
mutable Packet packet;
bool driverMode;
double pipelineIndex;
mutable LEDMode mode;
};
} // namespace photonlib

100
photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h Normal file
View File

@@ -0,0 +1,100 @@
/*
* 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/>.
*/
#pragma once
#include <string>
#include <frc/DriverStation.h>
#include <units/time.h>
#include <wpi/ArrayRef.h>
#include <wpi/SmallVector.h>
#include "photonlib/Packet.h"
#include "photonlib/PhotonTrackedTarget.h"
namespace photonlib {
/**
* Represents a pipeline result from a PhotonCamera.
*/
class PhotonPipelineResult {
public:
/**
* Constructs an empty pipeline result.
*/
PhotonPipelineResult() = default;
/**
* Constructs a pipeline result.
* @param latency The latency in the pipeline.
* @param targets The list of targets identified by the pipeline.
*/
PhotonPipelineResult(units::second_t latency,
wpi::ArrayRef<PhotonTrackedTarget> targets);
/**
* Returns the best target in this pipeline result. If there are no targets,
* this method will return an empty target with all values set to zero. The
* best target is determined by the target sort mode in the PhotonVision UI.
*
* @return The best target of the pipeline result.
*/
PhotonTrackedTarget GetBestTarget() const {
if (!HasTargets() && !HAS_WARNED) {
::frc::DriverStation::ReportError(
"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");
HAS_WARNED = true;
}
return hasTargets ? targets[0] : PhotonTrackedTarget();
}
/**
* Returns the latency in the pipeline.
* @return The latency in the pipeline.
*/
units::second_t GetLatency() const { return latency; }
/**
* Returns whether the pipeline has targets.
* @return Whether the pipeline has targets.
*/
bool HasTargets() const { return hasTargets; }
/**
* Returns a reference to the vector of targets.
* @return A reference to the vector of targets.
*/
const wpi::ArrayRef<PhotonTrackedTarget> GetTargets() const {
return targets;
}
bool operator==(const PhotonPipelineResult& other) const;
bool operator!=(const PhotonPipelineResult& other) const;
friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result);
friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result);
private:
units::second_t latency;
bool hasTargets;
wpi::SmallVector<PhotonTrackedTarget, 10> targets;
inline static bool HAS_WARNED = false;
};
} // namespace photonlib

93
photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h Normal file
View File

@@ -0,0 +1,93 @@
/*
* 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/>.
*/
#pragma once
#include <cstddef>
#include <string>
#include <vector>
#include <frc/geometry/Transform2d.h>
#include "photonlib/Packet.h"
namespace photonlib {
/**
* Represents a tracked target within a pipeline.
*/
class PhotonTrackedTarget {
public:
/**
* Constructs an empty target.
*/
PhotonTrackedTarget() = default;
/**
* Constructs a target.
* @param yaw The yaw of the target.
* @param pitch The pitch of the target.
* @param area The area of the target.
* @param skew The skew of the target.
* @param pose The camera-relative pose of the target.
*/
PhotonTrackedTarget(double yaw, double pitch, double area, double skew,
const frc::Transform2d& pose);
/**
* Returns the target yaw (positive-left).
* @return The target yaw.
*/
double GetYaw() const { return yaw; }
/**
* Returns the target pitch (positive-up)
* @return The target pitch.
*/
double GetPitch() const { return pitch; }
/**
* Returns the target area (0-100).
* @return The target area.
*/
double GetArea() const { return area; }
/**
* Returns the target skew (counter-clockwise positive).
* @return The target skew.
*/
double GetSkew() const { return skew; }
/**
* Returns the pose of the target relative to the robot.
* @return The pose of the target relative to the robot.
*/
frc::Transform2d GetCameraRelativePose() const { return cameraToTarget; }
bool operator==(const PhotonTrackedTarget& other) const;
bool operator!=(const PhotonTrackedTarget& other) const;
friend Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target);
friend Packet& operator>>(Packet& packet, PhotonTrackedTarget& target);
private:
double yaw = 0;
double pitch = 0;
double area = 0;
double skew = 0;
frc::Transform2d cameraToTarget;
};
} // namespace photonlib

173
photon-lib/src/main/native/include/photonlib/PhotonUtils.h Normal file
View File

@@ -0,0 +1,173 @@
/*
* 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/>.
*/
#pragma once
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/geometry/Transform2d.h>
#include <frc/geometry/Translation2d.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/math.h>
namespace photonlib {
class PhotonUtils {
public:
/**
* Algorithm from
* https://docs.limelightvision.io/en/latest/cs_estimating_distance.html
* Estimates range to a target using the target's elevation. This method can
* produce more stable results than SolvePNP when well tuned, if the full 6d
* robot pose is not required.
*
* @param cameraHeight The height of the camera off the floor.
* @param targetHeight The height of the target off the floor.
* @param cameraPitch The pitch of the camera from the horizontal plane.
* Positive valueBytes up.
* @param targetPitch The pitch of the target in the camera's lens. Positive
* values up.
* @return The estimated distance to the target.
*/
static units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight,
units::meter_t targetHeight,
units::radian_t cameraPitch,
units::radian_t targetPitch) {
return (targetHeight - cameraHeight) /
units::math::tan(cameraPitch + targetPitch);
}
/**
* Estimate the Translation2d of the target relative to the camera.
*
* @param targetDistance The distance to the target.
* @param yaw The observed yaw of the target.
*
* @return The target's camera-relative translation.
*/
static frc::Translation2d EstimateCameraToTargetTranslation(
units::meter_t targetDistance, const frc::Rotation2d& yaw) {
return {targetDistance * yaw.Cos(), targetDistance * yaw.Sin()};
}
/**
* Estimate the position of the robot in the field.
*
* @param cameraHeightMeters The physical height of the camera off the floor
* in meters.
* @param targetHeightMeters The physical height of the target off the floor
* in meters. This should be the height of whatever is being targeted (i.e. if
* the targeting region is set to top, this should be the height of the top of
* the target).
* @param cameraPitchRadians The pitch of the camera from the horizontal plane
* in radians. Positive values up.
* @param targetPitchRadians The pitch of the target in the camera's lens in
* radians. Positive values up.
* @param targetYaw The observed yaw of the target. Note that this
* *must* be CCW-positive, and Photon returns
* CW-positive.
* @param gyroAngle The current robot gyro angle, likely from
* odometry.
* @param fieldToTarget A frc::Pose2d representing the target position in
* the field coordinate system.
* @param cameraToRobot The position of the robot relative to the camera.
* If the camera was mounted 3 inches behind the
* "origin" (usually physical center) of the robot,
* this would be frc::Transform2d(3 inches, 0
* inches, 0 degrees).
* @return The position of the robot in the field.
*/
static frc::Pose2d EstimateFieldToRobot(
units::meter_t cameraHeight, units::meter_t targetHeight,
units::radian_t cameraPitch, units::radian_t targetPitch,
const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle,
const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot) {
return EstimateFieldToRobot(
EstimateCameraToTarget(
EstimateCameraToTargetTranslation(
CalculateDistanceToTarget(cameraHeight, targetHeight,
cameraPitch, targetPitch),
targetYaw),
fieldToTarget, gyroAngle),
fieldToTarget, cameraToRobot);
}
/**
* Estimates a {@link frc::Transform2d} that maps the camera position to the
* target position, using the robot's gyro. Note that the gyro angle provided
* *must* line up with the field coordinate system -- that is, it should read
* zero degrees when pointed towards the opposing alliance station, and
* increase as the robot rotates CCW.
*
* @param cameraToTargetTranslation A Translation2d that encodes the x/y
* position of the target relative to the
* camera.
* @param fieldToTarget A frc::Pose2d representing the target
* position in the field coordinate system.
* @param gyroAngle The current robot gyro angle, likely from
* odometry.
* @return A frc::Transform2d that takes us from the camera to the target.
*/
static frc::Transform2d EstimateCameraToTarget(
const frc::Translation2d& cameraToTargetTranslation,
const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle) {
// This pose maps our camera at the origin out to our target, in the robot
// reference frame
// The translation part of this frc::Transform2d is from the above step, and
// the rotation uses our robot's gyro.
return frc::Transform2d(cameraToTargetTranslation,
-gyroAngle - fieldToTarget.Rotation());
}
/**
* Estimates the pose of the robot in the field coordinate system, given the
* position of the target relative to the camera, the target relative to the
* field, and the robot relative to the camera.
*
* @param cameraToTarget The position of the target relative to the camera.
* @param fieldToTarget The position of the target in the field.
* @param cameraToRobot The position of the robot relative to the camera. If
* the camera was mounted 3 inches behind the "origin"
* (usually physical center) of the robot, this would be
* frc::Transform2d(3 inches, 0 inches, 0 degrees).
* @return The position of the robot in the field.
*/
static frc::Pose2d EstimateFieldToRobot(
const frc::Transform2d& cameraToTarget, const frc::Pose2d& fieldToTarget,
const frc::Transform2d& cameraToRobot) {
return EstimateFieldToCamera(cameraToTarget, fieldToTarget)
.TransformBy(cameraToRobot);
}
/**
* Estimates the pose of the camera in the field coordinate system, given the
* position of the target relative to the camera, and the target relative to
* the field. This *only* tracks the position of the camera, not the position
* of the robot itself.
*
* @param cameraToTarget The position of the target relative to the camera.
* @param fieldToTarget The position of the target in the field.
* @return The position of the camera in the field.
*/
static frc::Pose2d EstimateFieldToCamera(
const frc::Transform2d& cameraToTarget,
const frc::Pose2d& fieldToTarget) {
auto targetToCamera = cameraToTarget.Inverse();
return fieldToTarget.TransformBy(targetToCamera);
}
};
} // namespace photonlib

65
photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h Normal file
View File

@@ -0,0 +1,65 @@
/*
* 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/>.
*/
#pragma once
#include <memory>
#include <string>
#include <units/time.h>
#include <wpi/ArrayRef.h>
#include <wpi/SmallVector.h>
#include "photonlib/Packet.h"
#include "photonlib/PhotonCamera.h"
namespace photonlib {
/**
* Represents a camera that is connected to PhotonVision.ß
*/
class SimPhotonCamera : public PhotonCamera {
public:
/**
* Constructs a Simulated PhotonCamera from a root table.
*
* @param rootTable The root table that the camera is broadcasting information
* over.
*/
explicit SimPhotonCamera(std::shared_ptr<nt::NetworkTable> rootTable);
/**
* Constructs a Simulated PhotonCamera from the name of the camera.
*
* @param cameraName The nickname of the camera (found in the PhotonVision
* UI).
*/
explicit SimPhotonCamera(const std::string& cameraName);
/**
* Simulate one processed frame of vision data, putting one result to NT.
* @param latency Latency of frame processing
* @param tgtList Set of targets detected
*/
void SubmitProcessedFrame(units::second_t latency,
wpi::ArrayRef<PhotonTrackedTarget> tgtList);
private:
mutable Packet simPacket;
};
} // namespace photonlib

74
photon-lib/src/main/native/include/photonlib/SimVisionSystem.h Normal file
View File

@@ -0,0 +1,74 @@
/*
* 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/>.
*/
#pragma once
#include <string>
#include <vector>
#include <frc/geometry/Translation2d.h>
#include <units/angle.h>
#include <units/area.h>
#include <units/length.h>
#include <units/time.h>
#include <wpi/ArrayRef.h>
#include <wpi/SmallVector.h>
#include "photonlib/SimPhotonCamera.h"
#include "photonlib/SimVisionTarget.h"
namespace photonlib {
/**
* Represents a camera that is connected to PhotonVision.
*/
class SimVisionSystem {
public:
explicit SimVisionSystem(const std::string& name, units::degree_t camDiagFOV,
units::degree_t camPitch,
frc::Transform2d cameraToRobot,
units::meter_t cameraHeightOffGround,
units::meter_t maxLEDRange, int cameraResWidth,
int cameraResHeight, double minTargetArea);
void AddSimVisionTarget(SimVisionTarget tgt);
void MoveCamera(frc::Transform2d newcameraToRobot,
units::meter_t newCamHeight, units::degree_t newCamPitch);
void ProcessFrame(frc::Pose2d robotPose);
private:
units::degree_t camDiagFOV;
units::degree_t camPitch;
frc::Transform2d cameraToRobot;
units::meter_t cameraHeightOffGround;
units::meter_t maxLEDRange;
int cameraResWidth;
int cameraResHeight;
double minTargetArea;
units::degree_t camHorizFOV;
units::degree_t camVertFOV;
std::vector<SimVisionTarget> tgtList = {};
double GetM2PerPx(units::meter_t dist);
bool CamCanSeeTarget(units::meter_t distHypot, units::degree_t yaw,
units::degree_t pitch, double area);
public:
SimPhotonCamera cam = photonlib::SimPhotonCamera("Default");
};
} // namespace photonlib

45
photon-lib/src/main/native/include/photonlib/SimVisionTarget.h Normal file
View File

@@ -0,0 +1,45 @@
/*
* 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/>.
*/
#pragma once
#include <frc/geometry/Pose2d.h>
#include <units/area.h>
#include <units/length.h>
namespace photonlib {
/**
* Represents a target on the field which the vision processing system could
* detect.
*/
class SimVisionTarget {
public:
explicit SimVisionTarget(frc::Pose2d& targetPos,
units::meter_t targetHeightAboveGround,
units::meter_t targetWidth,
units::meter_t targetHeight);
frc::Pose2d targetPos;
units::meter_t targetHeightAboveGround;
units::meter_t targetWidth;
units::meter_t targetHeight;
double targetInfill_pct;
units::square_meter_t tgtArea;
};
} // namespace photonlib