mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-27 02:01:40 +00:00
Sim Updates for 2023 (#512)
* WIP updating sim stuff for 2023 and pose3d's * vision system build fixups, but test not yet passing. * WIP Sim fixups and working on testcases * Still doesn't work, but closer * tests pass * removed C++ sim support * formatting update * adjusted target height above ground per review * Turns out its unused * missed example removal
This commit is contained in:
Chris Gerth
@@ -25,9 +25,15 @@
|
||||
package org.photonvision;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Transform2d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
@@ -40,11 +46,14 @@ public class SimVisionSystem {
|
||||
double camVertFOVDegrees;
|
||||
double cameraHeightOffGroundMeters;
|
||||
double maxLEDRangeMeters;
|
||||
double camPitchDegrees;
|
||||
int cameraResWidth;
|
||||
int cameraResHeight;
|
||||
double minTargetArea;
|
||||
Transform2d cameraToRobot;
|
||||
Transform3d cameraToRobot;
|
||||
|
||||
Field2d dbgField;
|
||||
FieldObject2d dbgRobot;
|
||||
FieldObject2d dbgCamera;
|
||||
|
||||
ArrayList<SimVisionTarget> tgtList;
|
||||
|
||||
@@ -58,11 +67,7 @@ public class SimVisionSystem {
|
||||
* @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 cameraToRobot Transform to move from the camera's mount position to the robot's position
|
||||
* @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
|
||||
@@ -74,16 +79,12 @@ public class SimVisionSystem {
|
||||
public SimVisionSystem(
|
||||
String camName,
|
||||
double camDiagFOVDegrees,
|
||||
double camPitchDegrees,
|
||||
Transform2d cameraToRobot,
|
||||
double cameraHeightOffGroundMeters,
|
||||
Transform3d cameraToRobot,
|
||||
double maxLEDRangeMeters,
|
||||
int cameraResWidth,
|
||||
int cameraResHeight,
|
||||
double minTargetArea) {
|
||||
this.camPitchDegrees = camPitchDegrees;
|
||||
this.cameraToRobot = cameraToRobot;
|
||||
this.cameraHeightOffGroundMeters = cameraHeightOffGroundMeters;
|
||||
this.maxLEDRangeMeters = maxLEDRangeMeters;
|
||||
this.cameraResWidth = cameraResWidth;
|
||||
this.cameraResHeight = cameraResHeight;
|
||||
@@ -96,6 +97,11 @@ public class SimVisionSystem {
|
||||
|
||||
cam = new SimPhotonCamera(camName);
|
||||
tgtList = new ArrayList<>();
|
||||
|
||||
dbgField = new Field2d();
|
||||
dbgRobot = dbgField.getRobotObject();
|
||||
dbgCamera = dbgField.getObject(camName + " Camera");
|
||||
SmartDashboard.putData(camName + " Sim Field", dbgField);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -107,6 +113,10 @@ public class SimVisionSystem {
|
||||
*/
|
||||
public void addSimVisionTarget(SimVisionTarget target) {
|
||||
tgtList.add(target);
|
||||
dbgField
|
||||
.getObject("Target " + Integer.toString(target.targetID))
|
||||
.setPose(target.targetPose.toPose2d());
|
||||
;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -114,14 +124,9 @@ public class SimVisionSystem {
|
||||
* turret or some other mobile platform.
|
||||
*
|
||||
* @param newCameraToRobot New Transform 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) {
|
||||
public void moveCamera(Transform3d newCameraToRobot) {
|
||||
this.cameraToRobot = newCameraToRobot;
|
||||
this.cameraHeightOffGroundMeters = newCamHeightMeters;
|
||||
this.camPitchDegrees = newCamPitchDegrees;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -133,46 +138,76 @@ public class SimVisionSystem {
|
||||
* PhotonVision parameters.
|
||||
*/
|
||||
public void processFrame(Pose2d robotPoseMeters) {
|
||||
Pose2d cameraPos = robotPoseMeters.transformBy(cameraToRobot.inverse());
|
||||
var robotPose3d =
|
||||
new Pose3d(
|
||||
robotPoseMeters.getX(),
|
||||
robotPoseMeters.getY(),
|
||||
0.0,
|
||||
new Rotation3d(0, 0, robotPoseMeters.getRotation().getRadians()));
|
||||
processFrame(robotPose3d);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 in space. Will be used to calculate which
|
||||
* targets are actually in view, where they are at relative to the robot, and relevant
|
||||
* PhotonVision parameters.
|
||||
*/
|
||||
public void processFrame(Pose3d robotPoseMeters) {
|
||||
Pose3d cameraPose = robotPoseMeters.transformBy(cameraToRobot.inverse());
|
||||
|
||||
dbgRobot.setPose(robotPoseMeters.toPose2d());
|
||||
dbgCamera.setPose(cameraPose.toPose2d());
|
||||
|
||||
ArrayList<PhotonTrackedTarget> visibleTgtList = new ArrayList<>(tgtList.size());
|
||||
|
||||
tgtList.forEach(
|
||||
(tgt) -> {
|
||||
var camToTargetTrans = new Transform2d(cameraPos, tgt.targetPos);
|
||||
var camToTargetTrans = new Transform3d(cameraPose, tgt.targetPose);
|
||||
var t = camToTargetTrans.getTranslation();
|
||||
|
||||
double distAlongGroundMeters = camToTargetTrans.getTranslation().getNorm();
|
||||
double distVerticalMeters =
|
||||
tgt.targetHeightAboveGroundMeters - this.cameraHeightOffGroundMeters;
|
||||
double distMeters = Math.hypot(distAlongGroundMeters, distVerticalMeters);
|
||||
// Rough approximation of the alternate solution, which is (so far) always incorrect.
|
||||
var altTrans =
|
||||
new Translation3d(
|
||||
t.getX(),
|
||||
-1.0 * t.getY(),
|
||||
t.getZ()); // mirrored across camera axis in Y direction
|
||||
var altRot = camToTargetTrans.getRotation().times(-1.0); // flipped
|
||||
var camToTargetTransAlt = new Transform3d(altTrans, altRot);
|
||||
|
||||
double area = tgt.tgtAreaMeters2 / getM2PerPx(distAlongGroundMeters);
|
||||
double distMeters = t.getNorm();
|
||||
|
||||
double area_px = tgt.tgtAreaMeters2 / getM2PerPx(distMeters);
|
||||
|
||||
double yawDegrees = Units.radiansToDegrees(Math.atan2(t.getY(), t.getX()));
|
||||
|
||||
double camHeightAboveGround = cameraPose.getZ();
|
||||
double tgtHeightAboveGround = tgt.targetPose.getZ();
|
||||
double camPitchDegrees = Units.radiansToDegrees(cameraPose.getRotation().getY());
|
||||
|
||||
var transformAlongGround =
|
||||
new Transform2d(cameraPose.toPose2d(), tgt.targetPose.toPose2d());
|
||||
double distAlongGround = transformAlongGround.getTranslation().getNorm();
|
||||
|
||||
// 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;
|
||||
Units.radiansToDegrees(
|
||||
Math.atan2((tgtHeightAboveGround - camHeightAboveGround), distAlongGround))
|
||||
- camPitchDegrees;
|
||||
|
||||
if (camCanSeeTarget(distMeters, yawDegrees, pitchDegrees, area)) {
|
||||
if (camCanSeeTarget(distMeters, yawDegrees, pitchDegrees, area_px)) {
|
||||
// TODO simulate target corners
|
||||
visibleTgtList.add(
|
||||
new PhotonTrackedTarget(
|
||||
yawDegrees,
|
||||
pitchDegrees,
|
||||
area,
|
||||
area_px,
|
||||
0.0,
|
||||
-1, // TODO fiducial ID
|
||||
new Transform3d(),
|
||||
new Transform3d(),
|
||||
0.25,
|
||||
tgt.targetID,
|
||||
camToTargetTrans,
|
||||
camToTargetTransAlt,
|
||||
0.0, // TODO - simulate ambiguity when straight on?
|
||||
List.of(
|
||||
new TargetCorner(0, 0), new TargetCorner(0, 0),
|
||||
new TargetCorner(0, 0), new TargetCorner(0, 0))));
|
||||
|
||||
@@ -24,34 +24,28 @@
|
||||
|
||||
package org.photonvision;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
|
||||
public class SimVisionTarget {
|
||||
Pose2d targetPos;
|
||||
Pose3d targetPose;
|
||||
double targetWidthMeters;
|
||||
double targetHeightMeters;
|
||||
double targetHeightAboveGroundMeters;
|
||||
double tgtAreaMeters2;
|
||||
int targetID;
|
||||
|
||||
/**
|
||||
* 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 targetPos Pose3d of the target in field-relative coordinates
|
||||
* @param targetWidthMeters Width of the outer bounding box of the target in meters.
|
||||
* @param targetHeightMeters Pair Height of the outer bounding box of the target in meters.
|
||||
*/
|
||||
public SimVisionTarget(
|
||||
Pose2d targetPos,
|
||||
double targetHeightAboveGroundMeters,
|
||||
double targetWidthMeters,
|
||||
double targetHeightMeters) {
|
||||
this.targetPos = targetPos;
|
||||
this.targetHeightAboveGroundMeters = targetHeightAboveGroundMeters;
|
||||
Pose3d targetPos, double targetWidthMeters, double targetHeightMeters, int targetID) {
|
||||
this.targetPose = targetPos;
|
||||
this.targetWidthMeters = targetWidthMeters;
|
||||
this.targetHeightMeters = targetHeightMeters;
|
||||
this.tgtAreaMeters2 = targetWidthMeters * targetHeightMeters;
|
||||
this.targetID = targetID;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,51 +0,0 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2022 PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "photonlib/SimPhotonCamera.h"
|
||||
|
||||
namespace photonlib {
|
||||
|
||||
SimPhotonCamera::SimPhotonCamera(
|
||||
std::shared_ptr<nt::NetworkTableInstance> instance,
|
||||
const std::string& cameraName)
|
||||
: PhotonCamera(instance, cameraName) {}
|
||||
|
||||
SimPhotonCamera::SimPhotonCamera(const std::string& cameraName)
|
||||
: PhotonCamera(cameraName) {}
|
||||
|
||||
void SimPhotonCamera::SubmitProcessedFrame(
|
||||
units::second_t latency, wpi::span<const 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(std::string_view{simPacket.GetData().data(),
|
||||
simPacket.GetData().size()});
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace photonlib
|
||||
@@ -1,127 +0,0 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2022 PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "photonlib/SimVisionSystem.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <units/angle.h>
|
||||
#include <units/length.h>
|
||||
#include <wpi/span.h>
|
||||
|
||||
namespace photonlib {
|
||||
|
||||
SimVisionSystem::SimVisionSystem(const std::string& name,
|
||||
units::degree_t camDiagFOV,
|
||||
frc::Transform2d cameraToRobot,
|
||||
units::meter_t cameraHeightOffGround,
|
||||
units::meter_t maxLEDRange, int cameraResWidth,
|
||||
int cameraResHeight, double minTargetArea)
|
||||
: 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) {
|
||||
cameraToRobot = newCameraToRobot;
|
||||
cameraHeightOffGround = newCamHeight;
|
||||
}
|
||||
|
||||
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.value() / 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 = -units::math::atan2(
|
||||
camToTargetTrans.Translation().Y(), camToTargetTrans.Translation().X());
|
||||
units::degree_t pitchAngle =
|
||||
units::math::atan2(distVertical, distAlongGround);
|
||||
|
||||
auto translation = frc::Translation3d(camToTargetTrans.Translation().X(),
|
||||
camToTargetTrans.Translation().Y(),
|
||||
units::meter_t(0)); // TODO z height
|
||||
auto rotation = frc::Rotation3d(units::radian_t(0), pitchAngle, -yawAngle);
|
||||
frc::Transform3d camToTarget3d{translation, rotation};
|
||||
|
||||
if (CamCanSeeTarget(distHypot, yawAngle, pitchAngle, area)) {
|
||||
PhotonTrackedTarget newTgt = PhotonTrackedTarget(
|
||||
yawAngle.value(), pitchAngle.value(), area, 0.0, -1, camToTarget3d,
|
||||
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}});
|
||||
visibleTgtList.push_back(newTgt);
|
||||
}
|
||||
}
|
||||
|
||||
units::second_t procDelay(0.0); // Future - tie this to something meaningful
|
||||
cam.SubmitProcessedFrame(procDelay,
|
||||
wpi::span<PhotonTrackedTarget>(visibleTgtList));
|
||||
}
|
||||
|
||||
double SimVisionSystem::GetM2PerPx(units::meter_t dist) {
|
||||
double heightMPerPx =
|
||||
2 * dist.value() * units::math::tan(camVertFOV / 2) / cameraResHeight;
|
||||
double widthMPerPx =
|
||||
2 * dist.value() * 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
|
||||
@@ -1,40 +0,0 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2022 PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#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
|
||||
@@ -1,75 +0,0 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2022 PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include <units/time.h>
|
||||
#include <wpi/SmallVector.h>
|
||||
#include <wpi/span.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 instance The NetworkTableInstance to pull data from. This can be a
|
||||
* custom instance in simulation, but should *usually* be the default
|
||||
* NTInstance from {@link NetworkTableInstance::getDefault}
|
||||
* @param cameraName The name of the camera, as seen in the UI.
|
||||
*/
|
||||
explicit SimPhotonCamera(std::shared_ptr<nt::NetworkTableInstance> instance,
|
||||
const std::string& cameraName);
|
||||
|
||||
/**
|
||||
* 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::span<const PhotonTrackedTarget> tgtList);
|
||||
|
||||
private:
|
||||
mutable Packet simPacket;
|
||||
};
|
||||
|
||||
} // namespace photonlib
|
||||
@@ -1,77 +0,0 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2022 PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#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/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,
|
||||
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);
|
||||
void ProcessFrame(frc::Pose2d robotPose);
|
||||
|
||||
private:
|
||||
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
|
||||
@@ -1,51 +0,0 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2022 PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#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;
|
||||
units::square_meter_t tgtArea;
|
||||
};
|
||||
|
||||
} // namespace photonlib
|
||||
Reference in New Issue
Block a user