mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-25 01:41:40 +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
40
photon-lib/src/generate/photonlib.json.in
Normal file
40
photon-lib/src/generate/photonlib.json.in
Normal file
@@ -0,0 +1,40 @@
|
||||
{
|
||||
"fileName": "photonlib.json",
|
||||
"name": "photonlib",
|
||||
"version": "${photon_version}",
|
||||
"uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
|
||||
"mavenUrls": [
|
||||
"https://maven.photonvision.org/repository/internal"
|
||||
],
|
||||
"jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/lib/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision.lib",
|
||||
"artifactId": "PhotonLib-cpp",
|
||||
"version": "${photon_version}",
|
||||
"libName": "Photon",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": true,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"windowsx86-64",
|
||||
"linuxathena",
|
||||
"linuxx86-64",
|
||||
"osxx86-64"
|
||||
]
|
||||
}
|
||||
],
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "PhotonLib-java",
|
||||
"version": "${photon_version}"
|
||||
},
|
||||
{
|
||||
"groupId": "org.photonvision",
|
||||
"artifactId": "PhotonTargeting-java",
|
||||
"version": "${photon_version}"
|
||||
}
|
||||
]
|
||||
}
|
||||
46
photon-lib/src/main/driver/cpp/VendorJNI.cpp
Normal file
46
photon-lib/src/main/driver/cpp/VendorJNI.cpp
Normal 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
22
photon-lib/src/main/driver/cpp/driversource.cpp
Normal 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
22
photon-lib/src/main/driver/include/driverheader.h
Normal 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
4
photon-lib/src/main/driver/symbols.txt
Normal 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
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;
|
||||
}
|
||||
}
|
||||
90
photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
Normal file
90
photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
Normal 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
|
||||
@@ -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
|
||||
@@ -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
42
photon-lib/src/main/native/cpp/photonlib/SimPhotonCamera.cpp
Normal 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
119
photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp
Normal 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
33
photon-lib/src/main/native/cpp/photonlib/SimVisionTarget.cpp
Normal 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
121
photon-lib/src/main/native/include/photonlib/Packet.h
Normal 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
141
photon-lib/src/main/native/include/photonlib/PhotonCamera.h
Normal 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
|
||||
@@ -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
|
||||
@@ -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
173
photon-lib/src/main/native/include/photonlib/PhotonUtils.h
Normal 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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
98
photon-lib/src/test/java/org/photonvision/PacketTest.java
Normal file
98
photon-lib/src/test/java/org/photonvision/PacketTest.java
Normal file
@@ -0,0 +1,98 @@
|
||||
/*
|
||||
* 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.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Transform2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Translation2d;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import org.junit.jupiter.api.Assertions;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.photonvision.common.dataflow.structures.Packet;
|
||||
import org.photonvision.targeting.PhotonPipelineResult;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
|
||||
class PacketTest {
|
||||
@Test
|
||||
void testSimpleTrackedTarget() {
|
||||
var target =
|
||||
new PhotonTrackedTarget(
|
||||
3.0, 4.0, 9.0, -5.0, new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5)));
|
||||
var p = new Packet(PhotonTrackedTarget.PACK_SIZE_BYTES);
|
||||
target.populatePacket(p);
|
||||
|
||||
var b = new PhotonTrackedTarget();
|
||||
b.createFromPacket(p);
|
||||
|
||||
Assertions.assertEquals(target, b);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testSimplePipelineResult() {
|
||||
var result = new PhotonPipelineResult(1, new ArrayList<>());
|
||||
var p = new Packet(result.getPacketSize());
|
||||
result.populatePacket(p);
|
||||
|
||||
var b = new PhotonPipelineResult();
|
||||
b.createFromPacket(p);
|
||||
|
||||
Assertions.assertEquals(result, b);
|
||||
|
||||
var result2 =
|
||||
new PhotonPipelineResult(
|
||||
2,
|
||||
List.of(
|
||||
new PhotonTrackedTarget(
|
||||
3.0,
|
||||
-4.0,
|
||||
9.0,
|
||||
4.0,
|
||||
new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5))),
|
||||
new PhotonTrackedTarget(
|
||||
3.0,
|
||||
-4.0,
|
||||
9.1,
|
||||
6.7,
|
||||
new Transform2d(new Translation2d(1, 5), new Rotation2d(1.5)))));
|
||||
var p2 = new Packet(result2.getPacketSize());
|
||||
result2.populatePacket(p2);
|
||||
|
||||
var b2 = new PhotonPipelineResult();
|
||||
b2.createFromPacket(p2);
|
||||
|
||||
Assertions.assertEquals(result2, b2);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testBytePackFromCpp() {
|
||||
byte[] bytePack = {
|
||||
64, 8, 0, 0, 0, 0, 0, 0, 64, 16, 0, 0, 0, 0, 0, 0, 64, 34, 0, 0, 0, 0, 0, 0, -64, 20, 0, 0, 0,
|
||||
0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, 0, 0, 0, 0, 64, 85, 124, 101, 19, -54, -47,
|
||||
122
|
||||
};
|
||||
var t = new PhotonTrackedTarget();
|
||||
t.createFromPacket(new Packet(bytePack));
|
||||
|
||||
var target =
|
||||
new PhotonTrackedTarget(
|
||||
3.0, 4.0, 9.0, -5.0, new Transform2d(new Translation2d(1, 2), new Rotation2d(1.5)));
|
||||
|
||||
Assertions.assertEquals(t, target);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
/*
|
||||
* 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 org.junit.jupiter.api.Assertions;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.photonvision.common.dataflow.structures.Packet;
|
||||
import org.photonvision.targeting.PhotonPipelineResult;
|
||||
|
||||
class PhotonCameraTest {
|
||||
@Test
|
||||
public void testEmpty() {
|
||||
Assertions.assertDoesNotThrow(
|
||||
() -> {
|
||||
var packet = new Packet(1);
|
||||
var ret = new PhotonPipelineResult();
|
||||
packet.setData(new byte[0]);
|
||||
if (packet.getSize() < 1) {
|
||||
return;
|
||||
}
|
||||
ret.createFromPacket(packet);
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
* 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.util.Units;
|
||||
import org.junit.jupiter.api.Assertions;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class PhotonUtilTest {
|
||||
@Test
|
||||
public void testDistance() {
|
||||
var camHeight = 1;
|
||||
var targetHeight = 3;
|
||||
var camPitch = Units.degreesToRadians(0);
|
||||
var targetPitch = Units.degreesToRadians(30);
|
||||
|
||||
var dist =
|
||||
PhotonUtils.calculateDistanceToTargetMeters(camHeight, targetHeight, camPitch, targetPitch);
|
||||
|
||||
Assertions.assertEquals(3.464, dist, 0.01);
|
||||
|
||||
camHeight = 1;
|
||||
targetHeight = 2;
|
||||
camPitch = Units.degreesToRadians(20);
|
||||
targetPitch = Units.degreesToRadians(-10);
|
||||
|
||||
dist =
|
||||
PhotonUtils.calculateDistanceToTargetMeters(camHeight, targetHeight, camPitch, targetPitch);
|
||||
Assertions.assertEquals(5.671, dist, 0.01);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testTransform() {
|
||||
|
||||
var camHeight = 1;
|
||||
var tgtHeight = 3;
|
||||
var camPitch = 0;
|
||||
var tgtPitch = Units.degreesToRadians(30);
|
||||
var tgtYaw = new Rotation2d();
|
||||
var gyroAngle = new Rotation2d();
|
||||
var fieldToTarget = new Pose2d();
|
||||
var cameraToRobot = new Transform2d();
|
||||
|
||||
var fieldToRobot =
|
||||
PhotonUtils.estimateFieldToRobot(
|
||||
PhotonUtils.estimateCameraToTarget(
|
||||
PhotonUtils.estimateCameraToTargetTranslation(
|
||||
PhotonUtils.calculateDistanceToTargetMeters(
|
||||
camHeight, tgtHeight, camPitch, tgtPitch),
|
||||
tgtYaw),
|
||||
fieldToTarget,
|
||||
gyroAngle),
|
||||
fieldToTarget,
|
||||
cameraToRobot);
|
||||
|
||||
Assertions.assertEquals(-3.464, fieldToRobot.getX(), 0.1);
|
||||
Assertions.assertEquals(0, fieldToRobot.getY(), 0.1);
|
||||
Assertions.assertEquals(0, fieldToRobot.getRotation().getDegrees(), 0.1);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,307 @@
|
||||
/*
|
||||
* 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 static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertFalse;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
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;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import java.util.List;
|
||||
import java.util.stream.Stream;
|
||||
import org.junit.jupiter.api.Assertions;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.junit.jupiter.params.ParameterizedTest;
|
||||
import org.junit.jupiter.params.provider.Arguments;
|
||||
import org.junit.jupiter.params.provider.MethodSource;
|
||||
import org.junit.jupiter.params.provider.ValueSource;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
|
||||
class SimVisionSystemTest {
|
||||
@Test
|
||||
public void testEmpty() {
|
||||
Assertions.assertDoesNotThrow(
|
||||
() -> {
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 320, 240, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(new Pose2d(), 0.0, 1.0, 1.0));
|
||||
for (int loopIdx = 0; loopIdx < 100; loopIdx++) {
|
||||
sysUnderTest.processFrame(new Pose2d());
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
@ParameterizedTest
|
||||
@ValueSource(doubles = {5, 10, 15, 20, 25, 30})
|
||||
public void testDistanceAligned(double dist) {
|
||||
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 320, 240, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 1.0, 1.0));
|
||||
|
||||
final var robotPose = new Pose2d(new Translation2d(35 - dist, 0), new Rotation2d());
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
|
||||
var result = sysUnderTest.cam.getLatestResult();
|
||||
|
||||
assertTrue(result.hasTargets());
|
||||
assertEquals(result.getBestTarget().getCameraToTarget().getTranslation().getNorm(), dist);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testVisibilityCupidShuffle() {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3.0));
|
||||
|
||||
// To the right, to the right
|
||||
var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
// To the right, to the right
|
||||
robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-95));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
// To the left, to the left
|
||||
robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(90));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
// To the left, to the left
|
||||
robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(65));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
// now kick, now kick
|
||||
robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
// now kick, now kick
|
||||
robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
// now walk it by yourself
|
||||
robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-179));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
// now walk it by yourself
|
||||
sysUnderTest.moveCamera(
|
||||
new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180)), 0, 1.0);
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testNotVisibleVert1() {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3.0));
|
||||
|
||||
var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
sysUnderTest.moveCamera(new Transform2d(), 5000, 1.0); // vooop selfie stick
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testNotVisibleVert2() {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 45.0, new Transform2d(), 1, 99999, 1234, 1234, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 0.5));
|
||||
|
||||
var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
// Pitched back camera should mean target goes out of view below the robot as distance increases
|
||||
robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testNotVisibleTgtSize() {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 20.0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 0.1));
|
||||
|
||||
var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testNotVisibleTooFarForLEDs() {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 10, 640, 480, 1.0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 0.1));
|
||||
|
||||
var robotPose = new Pose2d(new Translation2d(28, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
}
|
||||
|
||||
@ParameterizedTest
|
||||
@ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23})
|
||||
public void testYawAngles(double testYaw) {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 4));
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 0.5, 0.5));
|
||||
|
||||
var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(testYaw));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
var res = sysUnderTest.cam.getLatestResult();
|
||||
assertTrue(res.hasTargets());
|
||||
var tgt = res.getBestTarget();
|
||||
assertEquals(tgt.getYaw(), testYaw, 0.0001);
|
||||
}
|
||||
|
||||
@ParameterizedTest
|
||||
@ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999})
|
||||
public void testCameraPitch(double testPitch) {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 4));
|
||||
final var robotPose = new Pose2d(new Translation2d(30, 0), new Rotation2d(0));
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 0.0, 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 0.5, 0.5));
|
||||
|
||||
sysUnderTest.moveCamera(new Transform2d(), 0.0, testPitch);
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
var res = sysUnderTest.cam.getLatestResult();
|
||||
assertTrue(res.hasTargets());
|
||||
var tgt = res.getBestTarget();
|
||||
// If the camera is pitched down by 10 degrees, the target should appear
|
||||
// in the upper part of the image (ie, pitch positive). Therefor,
|
||||
// pass/fail involves -1.0.
|
||||
assertEquals(tgt.getPitch(), -1.0 * testPitch, 0.0001);
|
||||
}
|
||||
|
||||
private static Stream<Arguments> distCalCParamProvider() {
|
||||
// Arbitrary and fairly random assortment of distances, camera pitches, and heights
|
||||
return Stream.of(
|
||||
Arguments.of(5, 35, 0),
|
||||
Arguments.of(6, 35, 1),
|
||||
Arguments.of(10, 35, 0),
|
||||
Arguments.of(15, 35, 2),
|
||||
Arguments.of(19.95, 35, 0),
|
||||
Arguments.of(20, 35, 0),
|
||||
Arguments.of(5, 42, 1),
|
||||
Arguments.of(6, 42, 0),
|
||||
Arguments.of(10, 42, 2),
|
||||
Arguments.of(15, 42, 0.5),
|
||||
Arguments.of(19.42, 35, 0),
|
||||
Arguments.of(20, 42, 0),
|
||||
Arguments.of(5, 55, 2),
|
||||
Arguments.of(6, 55, 0),
|
||||
Arguments.of(10, 54, 2.2),
|
||||
Arguments.of(15, 53, 0),
|
||||
Arguments.of(19.52, 35, 1.1),
|
||||
Arguments.of(20, 51, 2.87),
|
||||
Arguments.of(20, 55, 3));
|
||||
}
|
||||
|
||||
@ParameterizedTest
|
||||
@MethodSource("distCalCParamProvider")
|
||||
public void testDistanceCalc(double testDist, double testPitch, double testHeight) {
|
||||
// Assume dist along ground and tgt height the same. Iterate over other parameters.
|
||||
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 42));
|
||||
final var robotPose = new Pose2d(new Translation2d(35 - testDist, 0), new Rotation2d(0));
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem(
|
||||
"absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!",
|
||||
160.0,
|
||||
testPitch,
|
||||
new Transform2d(),
|
||||
testHeight,
|
||||
99999,
|
||||
640,
|
||||
480,
|
||||
0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, testDist, 0.5, 0.5));
|
||||
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
var res = sysUnderTest.cam.getLatestResult();
|
||||
assertTrue(res.hasTargets());
|
||||
var tgt = res.getBestTarget();
|
||||
assertEquals(tgt.getYaw(), 0.0, 0.0001);
|
||||
double distMeas =
|
||||
PhotonUtils.calculateDistanceToTargetMeters(
|
||||
testHeight,
|
||||
testDist,
|
||||
Units.degreesToRadians(testPitch),
|
||||
Units.degreesToRadians(tgt.getPitch()));
|
||||
assertEquals(distMeas, testDist, 0.001);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testMultipleTargets() {
|
||||
final var targetPoseL = new Pose2d(new Translation2d(35, 2), new Rotation2d());
|
||||
final var targetPoseC = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
final var targetPoseR = new Pose2d(new Translation2d(35, -2), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 160.0, 0.0, new Transform2d(), 5.0, 99999, 640, 480, 20.0);
|
||||
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 0.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 1.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 2.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 3.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 4.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 5.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 6.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 7.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 8.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 9.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 10.0, 0.25, 0.1));
|
||||
|
||||
var robotPose = new Pose2d(new Translation2d(30, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
var res = sysUnderTest.cam.getLatestResult();
|
||||
assertTrue(res.hasTargets());
|
||||
List<PhotonTrackedTarget> tgtList;
|
||||
tgtList = res.getTargets();
|
||||
assertEquals(tgtList.size(), 11);
|
||||
}
|
||||
}
|
||||
91
photon-lib/src/test/native/cpp/PacketTest.cpp
Normal file
91
photon-lib/src/test/native/cpp/PacketTest.cpp
Normal file
@@ -0,0 +1,91 @@
|
||||
/*
|
||||
* 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 <iostream>
|
||||
|
||||
#include <units/angle.h>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "photonlib/PhotonPipelineResult.h"
|
||||
#include "photonlib/PhotonTrackedTarget.h"
|
||||
|
||||
TEST(PacketTest, PhotonTrackedTarget) {
|
||||
photonlib::PhotonTrackedTarget target{
|
||||
3.0, 4.0, 9.0, -5.0,
|
||||
frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)};
|
||||
photonlib::Packet p;
|
||||
p << target;
|
||||
|
||||
photonlib::PhotonTrackedTarget b;
|
||||
p >> b;
|
||||
|
||||
for (auto& c : p.GetData()) {
|
||||
std::cout << static_cast<int>(c) << ",";
|
||||
}
|
||||
|
||||
EXPECT_EQ(target, b);
|
||||
}
|
||||
|
||||
TEST(PacketTest, PhotonPipelineResult) {
|
||||
photonlib::PhotonPipelineResult result{1_s, {}};
|
||||
photonlib::Packet p;
|
||||
p << result;
|
||||
|
||||
photonlib::PhotonPipelineResult b;
|
||||
p >> b;
|
||||
|
||||
EXPECT_EQ(result, b);
|
||||
|
||||
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
|
||||
photonlib::PhotonTrackedTarget{
|
||||
3.0, -4.0, 9.0, 4.0,
|
||||
frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)},
|
||||
photonlib::PhotonTrackedTarget{
|
||||
3.0, -4.0, 9.1, 6.7,
|
||||
frc::Transform2d(frc::Translation2d(1_m, 5_m), 1.5_rad)}};
|
||||
|
||||
photonlib::PhotonPipelineResult result2{2_s, targets};
|
||||
photonlib::Packet p2;
|
||||
p2 << result2;
|
||||
|
||||
photonlib::PhotonPipelineResult b2;
|
||||
p2 >> b2;
|
||||
|
||||
EXPECT_EQ(result2, b2);
|
||||
}
|
||||
|
||||
TEST(PacketTest, BytePackFromJava) {
|
||||
std::vector<signed char> bytePack{
|
||||
64, 8, 0, 0, 0, 0, 0, 0, 64, 16, 0, 0, 0, 0,
|
||||
0, 0, 64, 34, 0, 0, 0, 0, 0, 0, -64, 20, 0, 0,
|
||||
0, 0, 0, 0, 63, -16, 0, 0, 0, 0, 0, 0, 64, 0,
|
||||
0, 0, 0, 0, 0, 0, 64, 85, 124, 101, 19, -54, -47, 122};
|
||||
|
||||
std::vector<char> bytes;
|
||||
for (auto a : bytePack) bytes.emplace_back(static_cast<char>(a));
|
||||
|
||||
photonlib::Packet packet{bytes};
|
||||
|
||||
photonlib::PhotonTrackedTarget res;
|
||||
packet >> res;
|
||||
|
||||
photonlib::PhotonTrackedTarget target{
|
||||
3.0, 4.0, 9.0, -5.0,
|
||||
frc::Transform2d(frc::Translation2d(1_m, 2_m), 1.5_rad)};
|
||||
|
||||
EXPECT_EQ(res, target);
|
||||
}
|
||||
21
photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp
Normal file
21
photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp
Normal file
@@ -0,0 +1,21 @@
|
||||
/*
|
||||
* 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 "gtest/gtest.h"
|
||||
#include "photonlib/PhotonUtils.h"
|
||||
|
||||
TEST(PhotonUtilsTest, TestInclude) {}
|
||||
401
photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp
Normal file
401
photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp
Normal file
@@ -0,0 +1,401 @@
|
||||
/*
|
||||
* 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 <networktables/NetworkTable.h>
|
||||
#include <networktables/NetworkTableEntry.h>
|
||||
#include <networktables/NetworkTableInstance.h>
|
||||
|
||||
#include <units/angle.h>
|
||||
#include <units/length.h>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "photonlib/PhotonCamera.h"
|
||||
#include "photonlib/PhotonUtils.h"
|
||||
#include "photonlib/SimVisionSystem.h"
|
||||
|
||||
TEST(SimVisionSystemTest, testEmpty) {
|
||||
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||||
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||||
320, 240, 0.0);
|
||||
|
||||
for (int loopIdx = 0; loopIdx < 100; loopIdx++) {
|
||||
sysUnderTest.ProcessFrame(frc::Pose2d());
|
||||
}
|
||||
}
|
||||
|
||||
class SimVisionSystemTestDistParam : public testing::TestWithParam<double> {};
|
||||
INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestDistParamInst,
|
||||
SimVisionSystemTestDistParam,
|
||||
testing::Values(5, 10, 15, 20, 25, 30));
|
||||
|
||||
TEST_P(SimVisionSystemTestDistParam, testDistanceAligned) {
|
||||
double dist = GetParam();
|
||||
|
||||
auto targetPose =
|
||||
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||||
|
||||
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||||
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||||
320, 240, 0.0);
|
||||
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPose, 0.0_m, 1.0_m, 1.0_m));
|
||||
|
||||
auto robotPose = frc::Pose2d(
|
||||
frc::Translation2d(units::meter_t(35.0 - dist), 0_m), frc::Rotation2d());
|
||||
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
auto result = sysUnderTest.cam.GetLatestResult();
|
||||
ASSERT_TRUE(result.HasTargets());
|
||||
ASSERT_EQ(result.GetBestTarget()
|
||||
.GetCameraRelativePose()
|
||||
.Translation()
|
||||
.Norm()
|
||||
.to<double>(),
|
||||
dist);
|
||||
}
|
||||
|
||||
TEST(SimVisionSystemTest, testVisibilityCupidShuffle) {
|
||||
auto targetPose =
|
||||
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||||
|
||||
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||||
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||||
320, 240, 0.0);
|
||||
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPose, 0.0_m, 3.0_m, 3.0_m));
|
||||
|
||||
// To the right, to the right
|
||||
auto robotPose =
|
||||
frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(-70.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
auto result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_FALSE(result.HasTargets());
|
||||
|
||||
// To the right, to the right
|
||||
robotPose =
|
||||
frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(-95.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_FALSE(result.HasTargets());
|
||||
|
||||
// To the left, to the left
|
||||
robotPose =
|
||||
frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(90.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_FALSE(result.HasTargets());
|
||||
|
||||
// To the left, to the left
|
||||
robotPose =
|
||||
frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(65.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_FALSE(result.HasTargets());
|
||||
|
||||
// now kick, now kick
|
||||
robotPose =
|
||||
frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_TRUE(result.HasTargets());
|
||||
|
||||
// now kick, now kick
|
||||
robotPose =
|
||||
frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m), frc::Rotation2d(-5.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_TRUE(result.HasTargets());
|
||||
|
||||
// now walk it by yourself
|
||||
robotPose = frc::Pose2d(frc::Translation2d(2.0_m, 0.0_m),
|
||||
frc::Rotation2d(-179.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_FALSE(result.HasTargets());
|
||||
|
||||
// now walk it by yourself
|
||||
sysUnderTest.MoveCamera(
|
||||
frc::Transform2d(frc::Translation2d(), frc::Rotation2d(180_deg)), 0.0_m,
|
||||
1.0_deg);
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_TRUE(result.HasTargets());
|
||||
}
|
||||
|
||||
TEST(SimVisionSystemTest, testNotVisibleVert1) {
|
||||
auto targetPose =
|
||||
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||||
|
||||
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||||
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||||
640, 480, 0.0);
|
||||
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPose, 1.0_m, 3.0_m, 2.0_m));
|
||||
|
||||
auto robotPose =
|
||||
frc::Pose2d(frc::Translation2d(5.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||||
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
auto result = sysUnderTest.cam.GetLatestResult();
|
||||
ASSERT_TRUE(result.HasTargets());
|
||||
|
||||
sysUnderTest.MoveCamera(
|
||||
frc::Transform2d(frc::Translation2d(), frc::Rotation2d(0_deg)), 5000.0_m,
|
||||
1.0_deg);
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_FALSE(result.HasTargets());
|
||||
}
|
||||
|
||||
TEST(SimVisionSystemTest, testNotVisibleVert2) {
|
||||
auto targetPose =
|
||||
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||||
|
||||
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 45.0_deg,
|
||||
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||||
1234, 1234, 0.5);
|
||||
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPose, 3.0_m, 0.5_m, 0.5_m));
|
||||
|
||||
auto robotPose =
|
||||
frc::Pose2d(frc::Translation2d(32.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||||
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
auto result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_TRUE(result.HasTargets());
|
||||
|
||||
robotPose =
|
||||
frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_FALSE(result.HasTargets());
|
||||
}
|
||||
|
||||
TEST(SimVisionSystemTest, testNotVisibleTgtSize) {
|
||||
auto targetPose =
|
||||
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||||
|
||||
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||||
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||||
640, 480, 20.0);
|
||||
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPose, 1.10_m, 0.25_m, 0.1_m));
|
||||
|
||||
auto robotPose =
|
||||
frc::Pose2d(frc::Translation2d(32.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
auto result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_TRUE(result.HasTargets());
|
||||
|
||||
robotPose =
|
||||
frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_FALSE(result.HasTargets());
|
||||
}
|
||||
|
||||
TEST(SimVisionSystemTest, testNotVisibleTooFarForLEDs) {
|
||||
auto targetPose =
|
||||
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||||
|
||||
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||||
frc::Transform2d(), 1.0_m, 10.0_m,
|
||||
640, 480, 1.0);
|
||||
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPose, 1.10_m, 0.25_m, 0.1_m));
|
||||
|
||||
auto robotPose =
|
||||
frc::Pose2d(frc::Translation2d(28.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
auto result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_TRUE(result.HasTargets());
|
||||
|
||||
robotPose =
|
||||
frc::Pose2d(frc::Translation2d(0.0_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
result = sysUnderTest.cam.GetLatestResult();
|
||||
EXPECT_FALSE(result.HasTargets());
|
||||
}
|
||||
|
||||
class SimVisionSystemTestYawParam : public testing::TestWithParam<double> {};
|
||||
INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestYawParamInst,
|
||||
SimVisionSystemTestYawParam,
|
||||
testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23));
|
||||
TEST_P(SimVisionSystemTestYawParam, testYawAngles) {
|
||||
double testYaw = GetParam(); // Nope, Chuck testYaw
|
||||
auto targetPose =
|
||||
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d(45_deg));
|
||||
|
||||
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||||
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||||
640, 480, 0.0);
|
||||
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPose, 0.0_m, 0.5_m, 0.5_m));
|
||||
|
||||
auto robotPose = frc::Pose2d(frc::Translation2d(32_m, 0.0_m),
|
||||
frc::Rotation2d(units::degree_t(testYaw)));
|
||||
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
auto result = sysUnderTest.cam.GetLatestResult();
|
||||
ASSERT_TRUE(result.HasTargets());
|
||||
auto tgt = result.GetBestTarget();
|
||||
EXPECT_DOUBLE_EQ(tgt.GetYaw(), testYaw);
|
||||
}
|
||||
|
||||
class SimVisionSystemTestCameraPitchParam
|
||||
: public testing::TestWithParam<double> {};
|
||||
INSTANTIATE_TEST_SUITE_P(SimVisionSystemTestCameraPitchParamInst,
|
||||
SimVisionSystemTestCameraPitchParam,
|
||||
testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23,
|
||||
20.21, -19.999));
|
||||
TEST_P(SimVisionSystemTestCameraPitchParam, testCameraPitch) {
|
||||
double testPitch = GetParam();
|
||||
auto targetPose =
|
||||
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d(45_deg));
|
||||
|
||||
auto robotPose =
|
||||
frc::Pose2d(frc::Translation2d(30_m, 0.0_m), frc::Rotation2d(0.0_deg));
|
||||
|
||||
photonlib::SimVisionSystem sysUnderTest("Test", 80.0_deg, 0.0_deg,
|
||||
frc::Transform2d(), 1.0_m, 99999.0_m,
|
||||
640, 480, 0.0);
|
||||
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPose, 0.0_m, 0.5_m, 0.5_m));
|
||||
|
||||
sysUnderTest.MoveCamera(
|
||||
frc::Transform2d(frc::Translation2d(), frc::Rotation2d()), 0.0_m,
|
||||
units::degree_t(testPitch));
|
||||
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
auto result = sysUnderTest.cam.GetLatestResult();
|
||||
ASSERT_TRUE(result.HasTargets());
|
||||
auto tgt = result.GetBestTarget();
|
||||
// If the camera is pitched down by 10 degrees, the target should appear
|
||||
// in the upper part of the image (ie, pitch positive). Therefor,
|
||||
// pass/fail involves -1.0.
|
||||
EXPECT_DOUBLE_EQ(tgt.GetPitch(), -1.0 * testPitch);
|
||||
}
|
||||
|
||||
class SimVisionSystemTestDistCalcParam
|
||||
: public testing::TestWithParam<std::tuple<double, double, double>> {};
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
SimVisionSystemTestDistCalcParamInst, SimVisionSystemTestDistCalcParam,
|
||||
testing::Values(std::tuple<double, double, double>(5, 35, 0),
|
||||
std::tuple<double, double, double>(6, 35, 1),
|
||||
std::tuple<double, double, double>(10, 35, 0),
|
||||
std::tuple<double, double, double>(15, 35, 2),
|
||||
std::tuple<double, double, double>(19.95, 35, 0),
|
||||
std::tuple<double, double, double>(20, 35, 0),
|
||||
std::tuple<double, double, double>(5, 42, 1),
|
||||
std::tuple<double, double, double>(6, 42, 0),
|
||||
std::tuple<double, double, double>(10, 42, 2),
|
||||
std::tuple<double, double, double>(15, 42, 0.5),
|
||||
std::tuple<double, double, double>(19.42, 35, 0),
|
||||
std::tuple<double, double, double>(20, 42, 0),
|
||||
std::tuple<double, double, double>(5, 55, 2),
|
||||
std::tuple<double, double, double>(6, 55, 0),
|
||||
std::tuple<double, double, double>(10, 54, 2.2),
|
||||
std::tuple<double, double, double>(15, 53, 0),
|
||||
std::tuple<double, double, double>(19.52, 35, 1.1),
|
||||
std::tuple<double, double, double>(20, 51, 2.87),
|
||||
std::tuple<double, double, double>(20, 55, 3)));
|
||||
TEST_P(SimVisionSystemTestDistCalcParam, testDistanceCalc) {
|
||||
std::tuple<double, double, double> testArgs = GetParam();
|
||||
double testDist = std::get<0>(testArgs);
|
||||
double testPitch = std::get<1>(testArgs);
|
||||
double testHeight = std::get<2>(testArgs);
|
||||
|
||||
auto targetPose = frc::Pose2d(frc::Translation2d(35_m, 0_m),
|
||||
frc::Rotation2d(units::radian_t(3.14159 / 42)));
|
||||
|
||||
auto robotPose =
|
||||
frc::Pose2d(frc::Translation2d(units::meter_t(35 - testDist), 0.0_m),
|
||||
frc::Rotation2d(0.0_deg));
|
||||
|
||||
photonlib::SimVisionSystem sysUnderTest(
|
||||
"absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho"
|
||||
"wsyourdaygoingihopegoodhaveagreatrestofyourlife",
|
||||
160.0_deg, units::degree_t(testPitch), frc::Transform2d(),
|
||||
units::meter_t(testHeight), 99999.0_m, 640, 480, 0.0);
|
||||
|
||||
sysUnderTest.AddSimVisionTarget(photonlib::SimVisionTarget(
|
||||
targetPose, units::meter_t(testDist), 0.5_m, 0.5_m));
|
||||
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
auto result = sysUnderTest.cam.GetLatestResult();
|
||||
ASSERT_TRUE(result.HasTargets());
|
||||
auto tgt = result.GetBestTarget();
|
||||
EXPECT_DOUBLE_EQ(tgt.GetYaw(), 0.0);
|
||||
units::meter_t distMeas = photonlib::PhotonUtils::CalculateDistanceToTarget(
|
||||
units::meter_t(testHeight), units::meter_t(testDist),
|
||||
units::degree_t(testPitch), units::degree_t(tgt.GetPitch()));
|
||||
EXPECT_DOUBLE_EQ(distMeas.to<double>(), testDist);
|
||||
}
|
||||
|
||||
TEST(SimVisionSystemTest, testMultipleTargets) {
|
||||
auto targetPoseL =
|
||||
frc::Pose2d(frc::Translation2d(35_m, 2_m), frc::Rotation2d());
|
||||
auto targetPoseC =
|
||||
frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d());
|
||||
auto targetPoseR =
|
||||
frc::Pose2d(frc::Translation2d(35_m, -2_m), frc::Rotation2d());
|
||||
|
||||
photonlib::SimVisionSystem sysUnderTest("test", 160.0_deg, 0.0_deg,
|
||||
frc::Transform2d(), 5.0_m, 99999.0_m,
|
||||
640, 480, 20.0);
|
||||
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPoseL, 0.0_m, 0.25_m, 0.1_m));
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPoseC, 1.0_m, 0.25_m, 0.1_m));
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPoseR, 2.0_m, 0.25_m, 0.1_m));
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPoseL, 3.0_m, 0.25_m, 0.1_m));
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPoseC, 4.0_m, 0.25_m, 0.1_m));
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPoseR, 5.0_m, 0.25_m, 0.1_m));
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPoseL, 6.0_m, 0.25_m, 0.1_m));
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPoseC, 7.0_m, 0.25_m, 0.1_m));
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPoseL, 8.0_m, 0.25_m, 0.1_m));
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPoseR, 9.0_m, 0.25_m, 0.1_m));
|
||||
sysUnderTest.AddSimVisionTarget(
|
||||
photonlib::SimVisionTarget(targetPoseL, 10.0_m, 0.25_m, 0.1_m));
|
||||
|
||||
auto robotPose =
|
||||
frc::Pose2d(frc::Translation2d(30_m, 0.0_m), frc::Rotation2d(5.0_deg));
|
||||
|
||||
sysUnderTest.ProcessFrame(robotPose);
|
||||
auto result = sysUnderTest.cam.GetLatestResult();
|
||||
ASSERT_TRUE(result.HasTargets());
|
||||
|
||||
auto tgtList = result.GetTargets();
|
||||
EXPECT_EQ(11ul, tgtList.size());
|
||||
}
|
||||
24
photon-lib/src/test/native/cpp/main.cpp
Normal file
24
photon-lib/src/test/native/cpp/main.cpp
Normal file
@@ -0,0 +1,24 @@
|
||||
/*
|
||||
* 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 "gtest/gtest.h"
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
int ret = RUN_ALL_TESTS();
|
||||
return ret;
|
||||
}
|
||||
Reference in New Issue
Block a user