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
198
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
Normal file
198
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
Normal 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
166
photon-lib/src/main/java/org/photonvision/PhotonUtils.java
Normal 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);
|
||||
}
|
||||
}
|
||||
@@ -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
181
photon-lib/src/main/java/org/photonvision/SimVisionSystem.java
Normal 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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user