From b408a58e9eeaa56f29e7572a0394cbd44c1acb2e Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Thu, 3 Nov 2022 15:05:37 -0500 Subject: [PATCH] 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 --- .../org/photonvision/SimVisionSystem.java | 119 +++-- .../org/photonvision/SimVisionTarget.java | 20 +- .../native/cpp/photonlib/SimPhotonCamera.cpp | 51 --- .../native/cpp/photonlib/SimVisionSystem.cpp | 127 ------ .../native/cpp/photonlib/SimVisionTarget.cpp | 40 -- .../include/photonlib/SimPhotonCamera.h | 75 ---- .../include/photonlib/SimVisionSystem.h | 77 ---- .../include/photonlib/SimVisionTarget.h | 51 --- .../org/photonvision/SimVisionSystemTest.java | 259 +++++++---- .../test/native/cpp/SimVisionSystemTest.cpp | 416 ------------------ .../src/main/cpp/examples/examples.json | 12 - .../cpp/examples/simaimandrange/cpp/Robot.cpp | 73 --- .../simaimandrange/cpp/sim/DrivetrainSim.cpp | 50 --- .../simaimandrange/include/DrivetrainSim.h | 111 ----- .../examples/simaimandrange/include/Robot.h | 77 ---- .../simaimandrange/sim/DrivetrainSim.java | 22 +- .../examples/simposeest/robot/Constants.java | 4 +- .../simposeest/sim/DrivetrainSim.java | 6 +- 18 files changed, 276 insertions(+), 1314 deletions(-) delete mode 100644 photon-lib/src/main/native/cpp/photonlib/SimPhotonCamera.cpp delete mode 100644 photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp delete mode 100644 photon-lib/src/main/native/cpp/photonlib/SimVisionTarget.cpp delete mode 100644 photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h delete mode 100644 photon-lib/src/main/native/include/photonlib/SimVisionSystem.h delete mode 100644 photon-lib/src/main/native/include/photonlib/SimVisionTarget.h delete mode 100644 photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp delete mode 100644 photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/Robot.cpp delete mode 100644 photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/sim/DrivetrainSim.cpp delete mode 100644 photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/DrivetrainSim.h delete mode 100644 photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/Robot.h diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java index dda23e7ce..e90e4e5dc 100644 --- a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java @@ -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 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 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)))); diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java b/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java index 72f6aa9c1..582df3c05 100644 --- a/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java +++ b/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java @@ -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; } } diff --git a/photon-lib/src/main/native/cpp/photonlib/SimPhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/SimPhotonCamera.cpp deleted file mode 100644 index 6246276da..000000000 --- a/photon-lib/src/main/native/cpp/photonlib/SimPhotonCamera.cpp +++ /dev/null @@ -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 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 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 diff --git a/photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp b/photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp deleted file mode 100644 index aa1fc3d3d..000000000 --- a/photon-lib/src/main/native/cpp/photonlib/SimVisionSystem.cpp +++ /dev/null @@ -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 - -#include -#include -#include - -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 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(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 diff --git a/photon-lib/src/main/native/cpp/photonlib/SimVisionTarget.cpp b/photon-lib/src/main/native/cpp/photonlib/SimVisionTarget.cpp deleted file mode 100644 index 1a5eb5aaf..000000000 --- a/photon-lib/src/main/native/cpp/photonlib/SimVisionTarget.cpp +++ /dev/null @@ -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 diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h deleted file mode 100644 index e1b3df987..000000000 --- a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h +++ /dev/null @@ -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 -#include - -#include -#include -#include - -#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 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 tgtList); - - private: - mutable Packet simPacket; -}; - -} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h b/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h deleted file mode 100644 index d99bebb47..000000000 --- a/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h +++ /dev/null @@ -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 -#include - -#include -#include -#include -#include -#include -#include - -#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 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 diff --git a/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h b/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h deleted file mode 100644 index f7a8f466d..000000000 --- a/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h +++ /dev/null @@ -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 -#include -#include - -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 diff --git a/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java b/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java index a9cd24350..25ae30534 100644 --- a/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java +++ b/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java @@ -29,13 +29,19 @@ import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; import java.util.List; import java.util.stream.Stream; +import org.junit.jupiter.api.AfterAll; import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.Test; import org.junit.jupiter.params.ParameterizedTest; import org.junit.jupiter.params.provider.Arguments; @@ -49,18 +55,30 @@ class SimVisionSystemTest { 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)); + new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 320, 240, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(new Pose3d(), 1.0, 1.0, 42)); for (int loopIdx = 0; loopIdx < 100; loopIdx++) { sysUnderTest.processFrame(new Pose2d()); } }); } + @BeforeAll + public static void setUp() { + // NT live for debug purposes + NetworkTableInstance.getDefault().startServer(); + + // No version check for testing + PhotonCamera.setVersionCheckEnabled(false); + } + + @AfterAll + public static void shutDown() {} + // @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()); + // final var targetPose = new Pose2d(new Translation2d(15.98, 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)); @@ -76,10 +94,10 @@ class SimVisionSystemTest { @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)); + final var targetPose = + new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI)); + var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3)); // To the right, to the right var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70)); @@ -117,37 +135,40 @@ class SimVisionSystemTest { assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); // now walk it by yourself - sysUnderTest.moveCamera( - new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180)), 0, 1.0); + sysUnderTest.moveCamera(new Transform3d(new Translation3d(), new Rotation3d(0, 0, Math.PI))); 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)); + final var targetPose = + new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); + var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3)); - PhotonCamera.setVersionCheckEnabled(false); 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.moveCamera( + new Transform3d( + new Translation3d(0, 0, 5000), new Rotation3d(0, 0, Math.PI))); // 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()); + final var targetPose = + new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI)); + var robotToCamera = + new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, Math.PI / 4, 0)); 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)); + new SimVisionSystem("Test", 80.0, robotToCamera.inverse(), 99999, 1234, 1234, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 1736)); - var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(5)); + var robotPose = new Pose2d(new Translation2d(14.98, 0), Rotation2d.fromDegrees(5)); sysUnderTest.processFrame(robotPose); assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); @@ -159,12 +180,12 @@ class SimVisionSystemTest { @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)); + final var targetPose = + new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); + var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 20.0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.1, 0.025, 24)); - var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(5)); + var robotPose = new Pose2d(new Translation2d(12, 0), Rotation2d.fromDegrees(5)); sysUnderTest.processFrame(robotPose); assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); @@ -175,12 +196,12 @@ class SimVisionSystemTest { @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)); + final var targetPose = + new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); + var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 10, 640, 480, 1.0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 78)); - var robotPose = new Pose2d(new Translation2d(28, 0), Rotation2d.fromDegrees(5)); + var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(5)); sysUnderTest.processFrame(robotPose); assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); @@ -192,12 +213,12 @@ class SimVisionSystemTest { @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)); + final var targetPose = + new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, 3 * Math.PI / 4)); + var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 3)); - var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(testYaw)); + var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(-1.0 * testYaw)); sysUnderTest.processFrame(robotPose); var res = sysUnderTest.cam.getLatestResult(); assertTrue(res.hasTargets()); @@ -208,45 +229,48 @@ class SimVisionSystemTest { @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)); + final var targetPose = + new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4)); + final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0)); + var sysUnderTest = new SimVisionSystem("Test", 120.0, new Transform3d(), 99999, 640, 480, 0); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 23)); - sysUnderTest.moveCamera(new Transform2d(), 0.0, testPitch); + // Here, passing in a positive testPitch points the camera downward (since moveCamera takes the + // camera->robot transform) + sysUnderTest.moveCamera( + new Transform3d( + new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0))); 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(), -testPitch, 0.0001); + + // Since the camera is level with the target, a downward point will mean the target is in the + // upper half of the image + // which should produce positive pitch. + assertEquals(testPitch, tgt.getPitch(), 0.0001); } private static Stream 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, 15.98, 0), + Arguments.of(6, 15.98, 1), + Arguments.of(10, 15.98, 0), + Arguments.of(15, 15.98, 2), + Arguments.of(19.95, 15.98, 0), + Arguments.of(20, 15.98, 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(19.42, 15.98, 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)); + Arguments.of(19.52, 15.98, 1.1)); } @ParameterizedTest @@ -254,20 +278,25 @@ class SimVisionSystemTest { 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)); + final var targetPose = + new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI * 0.98)); + final var robotPose = + new Pose3d(new Translation3d(15.98 - Units.feetToMeters(testDist), 0, 0), new Rotation3d()); + final var robotToCamera = + new Transform3d( + new Translation3d(0, 0, Units.feetToMeters(testHeight)), + new Rotation3d(0, Units.degreesToRadians(testPitch), 0)); + var sysUnderTest = new SimVisionSystem( "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!", 160.0, - testPitch, - new Transform2d(), - testHeight, + robotToCamera.inverse(), 99999, 640, 480, 0); - sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, testDist, 0.5, 0.5)); + sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 0)); sysUnderTest.processFrame(robotPose); var res = sysUnderTest.cam.getLatestResult(); @@ -276,34 +305,102 @@ class SimVisionSystemTest { assertEquals(tgt.getYaw(), 0.0, 0.0001); double distMeas = PhotonUtils.calculateDistanceToTargetMeters( - testHeight, - testDist, + robotToCamera.getZ(), + targetPose.getZ(), Units.degreesToRadians(testPitch), Units.degreesToRadians(tgt.getPitch())); - assertEquals(distMeas, testDist, 0.001); + assertEquals(Units.feetToMeters(testDist), distMeas, 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); + final var targetPoseL = + new Pose3d(new Translation3d(15.98, 2, 0), new Rotation3d(0, 0, Math.PI)); + final var targetPoseC = + new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, Math.PI)); + final var targetPoseR = + new Pose3d(new Translation3d(15.98, -2, 0), new Rotation3d(0, 0, Math.PI)); + var sysUnderTest = new SimVisionSystem("Test", 160.0, new Transform3d(), 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)); + sysUnderTest.addSimVisionTarget( + new SimVisionTarget( + targetPoseL.transformBy( + new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), + 0.25, + 0.25, + 1)); + sysUnderTest.addSimVisionTarget( + new SimVisionTarget( + targetPoseC.transformBy( + new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), + 0.25, + 0.25, + 2)); + sysUnderTest.addSimVisionTarget( + new SimVisionTarget( + targetPoseR.transformBy( + new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), + 0.25, + 0.25, + 3)); + sysUnderTest.addSimVisionTarget( + new SimVisionTarget( + targetPoseL.transformBy( + new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), + 0.25, + 0.25, + 4)); + sysUnderTest.addSimVisionTarget( + new SimVisionTarget( + targetPoseC.transformBy( + new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), + 0.25, + 0.25, + 5)); + sysUnderTest.addSimVisionTarget( + new SimVisionTarget( + targetPoseR.transformBy( + new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), + 0.25, + 0.25, + 6)); + sysUnderTest.addSimVisionTarget( + new SimVisionTarget( + targetPoseL.transformBy( + new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())), + 0.25, + 0.25, + 7)); + sysUnderTest.addSimVisionTarget( + new SimVisionTarget( + targetPoseC.transformBy( + new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())), + 0.25, + 0.25, + 8)); + sysUnderTest.addSimVisionTarget( + new SimVisionTarget( + targetPoseL.transformBy( + new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())), + 0.25, + 0.25, + 9)); + sysUnderTest.addSimVisionTarget( + new SimVisionTarget( + targetPoseR.transformBy( + new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())), + 0.25, + 0.25, + 10)); + sysUnderTest.addSimVisionTarget( + new SimVisionTarget( + targetPoseL.transformBy( + new Transform3d(new Translation3d(0, 0, 0.25), new Rotation3d())), + 0.25, + 0.25, + 11)); - var robotPose = new Pose2d(new Translation2d(30, 0), Rotation2d.fromDegrees(5)); + var robotPose = new Pose2d(new Translation2d(6.0, 0), Rotation2d.fromDegrees(0.25)); sysUnderTest.processFrame(robotPose); var res = sysUnderTest.cam.getLatestResult(); assertTrue(res.hasTargets()); diff --git a/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp b/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp deleted file mode 100644 index 925172dbc..000000000 --- a/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp +++ /dev/null @@ -1,416 +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. - */ - -// So for now, with the new apriltag stuff, this is totally broken -// For now, commented out - -/* -#include -#include -#include -#include -#include - -#include "gtest/gtest.h" -#include "photonlib/PhotonCamera.h" -#include "photonlib/PhotonUtils.h" -#include "photonlib/SimVisionSystem.h" - -TEST(SimVisionSystemTest, Empty) { - photonlib::SimVisionSystem sysUnderTest("Test", 80.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 SimVisionSystemDistParamTest : public testing::TestWithParam {}; -INSTANTIATE_TEST_SUITE_P(SimVisionSystemDistParamTests, - SimVisionSystemDistParamTest, - testing::Values(5, 10, 15, 20, 25, 30)); - -TEST_P(SimVisionSystemDistParamTest, DistanceAligned) { - double dist = GetParam(); - - auto targetPose = - frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); - - photonlib::SimVisionSystem sysUnderTest("Test", 80.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()); - std::cout << "Best target pitch " << -result.GetBestTarget().GetCameraToTarget().Translation().X().value(); - - ASSERT_EQ(result.GetBestTarget() - .GetCameraToTarget() - .Translation() - .Norm() - .value(), - dist); -} - -TEST(SimVisionSystemTest, VisibilityCupidShuffle) { - auto targetPose = - frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); - - photonlib::SimVisionSystem sysUnderTest("Test", 80.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); - sysUnderTest.ProcessFrame(robotPose); - result = sysUnderTest.cam.GetLatestResult(); - EXPECT_TRUE(result.HasTargets()); -} - -TEST(SimVisionSystemTest, NotVisibleVert1) { - auto targetPose = - frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); - - photonlib::SimVisionSystem sysUnderTest("Test", 80.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); - sysUnderTest.ProcessFrame(robotPose); - result = sysUnderTest.cam.GetLatestResult(); - EXPECT_FALSE(result.HasTargets()); -} - -TEST(SimVisionSystemTest, NotVisibleVert2) { - auto targetPose = - frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); - - photonlib::SimVisionSystem sysUnderTest("Test", 80.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, NotVisibleTgtSize) { - auto targetPose = - frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); - - photonlib::SimVisionSystem sysUnderTest("Test", 80.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, NotVisibleTooFarForLEDs) { - auto targetPose = - frc::Pose2d(frc::Translation2d(35_m, 0_m), frc::Rotation2d()); - - photonlib::SimVisionSystem sysUnderTest("Test", 80.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 SimVisionSystemYawParamTest : public testing::TestWithParam {}; -INSTANTIATE_TEST_SUITE_P(SimVisionSystemYawParamTests, - SimVisionSystemYawParamTest, - testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23)); -TEST_P(SimVisionSystemYawParamTest, YawAngles) { - 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, - 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 SimVisionSystemCameraPitchParamTest -// : public testing::TestWithParam {}; -// INSTANTIATE_TEST_SUITE_P(SimVisionSystemCameraPitchParamTests, -// SimVisionSystemCameraPitchParamTest, -// testing::Values(-10, -5, -0, -1, -2, 5, 7, 10.23, -// 20.21, -19.999)); -// TEST_P(SimVisionSystemCameraPitchParamTest, CameraPitch) { -// 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, -// 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)); - -// photonlib::PhotonCamera::SetVersionCheckEnabled(false); -// 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(), -testPitch); -// } - -class SimVisionSystemDistCalcParamTest - : public testing::TestWithParam> {}; -INSTANTIATE_TEST_SUITE_P( - SimVisionSystemDistCalcParamTests, SimVisionSystemDistCalcParamTest, - testing::Values(std::tuple(5, 35, 0), - std::tuple(6, 35, 1), - std::tuple(10, 35, 0), - std::tuple(15, 35, 2), - std::tuple(19.95, 35, 0), - std::tuple(20, 35, 0), - std::tuple(5, 42, 1), - std::tuple(6, 42, 0), - std::tuple(10, 42, 2), - std::tuple(15, 42, 0.5), - std::tuple(19.42, 35, 0), - std::tuple(20, 42, 0), - std::tuple(5, 55, 2), - std::tuple(6, 55, 0), - std::tuple(10, 54, 2.2), - std::tuple(15, 53, 0), - std::tuple(19.52, 35, 1.1), - std::tuple(20, 51, 2.87), - std::tuple(20, 55, 3))); -// TEST_P(SimVisionSystemDistCalcParamTest, DistanceCalc) { -// std::tuple 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.value(), testDist); -// } - -TEST(SimVisionSystemTest, MultipleTargets) { - 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, - 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()); -} -*/ diff --git a/photonlib-cpp-examples/src/main/cpp/examples/examples.json b/photonlib-cpp-examples/src/main/cpp/examples/examples.json index 6344225ba..2460afe15 100644 --- a/photonlib-cpp-examples/src/main/cpp/examples/examples.json +++ b/photonlib-cpp-examples/src/main/cpp/examples/examples.json @@ -35,16 +35,4 @@ "dependencies": [], "foldername": "aimandrange" } - { - "name": "SimAimAndRange", - "description": "Adding Simulation Support to the Aim And Range example", - "tags": [], - "gradlebase": "cpp", - "language": "cpp", - "commandversion": 1, - "mainclass": "Main", - "packagetoreplace": null, - "dependencies": [], - "foldername": "simaimandrange" - } ] diff --git a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/Robot.cpp b/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/Robot.cpp deleted file mode 100644 index 969c64553..000000000 --- a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/Robot.cpp +++ /dev/null @@ -1,73 +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 "Robot.h" - -#include - -void Robot::TeleopPeriodic() { - double forwardSpeed; - double rotationSpeed; - - if (xboxController.GetAButton()) { - // Vision-alignment mode - // Query the latest result from PhotonVision - const auto& result = camera.GetLatestResult(); - - if (result.HasTargets()) { - // First calculate range - units::meter_t range = photonlib::PhotonUtils::CalculateDistanceToTarget( - CAMERA_HEIGHT, TARGET_HEIGHT, CAMERA_PITCH, - units::degree_t{result.GetBestTarget().GetPitch()}); - - // Use this range as the measurement we give to the PID controller. - // -1.0 required to ensure positive PID controller effort _increases_ - // range - forwardSpeed = -forwardController.Calculate(range.value(), - GOAL_RANGE_METERS.value()); - - // Also calculate angular power - // -1.0 required to ensure positive PID controller effort _increases_ yaw - rotationSpeed = - -turnController.Calculate(result.GetBestTarget().GetYaw(), 0); - } else { - // If we have no targets, stay still. - forwardSpeed = 0; - rotationSpeed = 0; - } - } else { - // Manual Driver Mode - forwardSpeed = -xboxController.GetRightY(); - rotationSpeed = xboxController.GetLeftX(); - } - - // Use our forward/turn speeds to control the drivetrain - drive.ArcadeDrive(forwardSpeed, rotationSpeed); -} - -void Robot::SimulationPeriodic() { dtSim.update(); } - -#ifndef RUNNING_FRC_TESTS -int main() { return frc::StartRobot(); } -#endif diff --git a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/sim/DrivetrainSim.cpp b/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/sim/DrivetrainSim.cpp deleted file mode 100644 index 484555679..000000000 --- a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/cpp/sim/DrivetrainSim.cpp +++ /dev/null @@ -1,50 +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 "DrivetrainSim.h" - -/** - * Perform all periodic drivetrain simulation related tasks to advance our - * simulation of robot physics forward by a single 20ms step. - */ -void DrivetrainSim::update() { - double leftMotorCmd = 0; - double rightMotorCmd = 0; - - if (frc::DriverStation::IsEnabled() && - !frc::RobotController::IsBrownedOut()) { - leftMotorCmd = leftLeader.GetSpeed(); - rightMotorCmd = rightLeader.GetSpeed(); - } - - m_drivetrainSimulator.SetInputs( - units::volt_t(leftMotorCmd * frc::RobotController::GetInputVoltage()), - units::volt_t(-rightMotorCmd * frc::RobotController::GetInputVoltage())); - m_drivetrainSimulator.Update(20_ms); - - // Update PhotonVision based on our new robot position. - simVision.ProcessFrame(m_drivetrainSimulator.GetPose()); - - field.SetRobotPose(m_drivetrainSimulator.GetPose()); -} diff --git a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/DrivetrainSim.h b/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/DrivetrainSim.h deleted file mode 100644 index 3cb00f903..000000000 --- a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/DrivetrainSim.h +++ /dev/null @@ -1,111 +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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#pragma once - -class DrivetrainSim { - public: - DrivetrainSim() { - simVision.AddSimVisionTarget(photonlib::SimVisionTarget( - farTargetPose, 81.91_in, targetWidth, targetHeight)); - frc::SmartDashboard::PutData("Field", &field); - } - - void update(); - - private: - // Simulated Motor Controllers - frc::sim::PWMSim leftLeader{0}; - frc::sim::PWMSim rightLeader{1}; - - // Simulation Physics - // Configure these to match your drivetrain's physical dimensions - // and characterization results. - static constexpr decltype(1_V / 1_mps) kv = 1.98 * 1_V * 1_s / 1_m; - static constexpr decltype(1_V / 1_mps_sq) ka = 0.2 * 1_V * 1_s * 1_s / 1_m; - static constexpr decltype(1_V / 1_rad_per_s) kvAngular = - 1.5 * 1_V * 1_s / 1_rad; - static constexpr decltype(1_V / 1_rad_per_s_sq) kaAngular = - 0.3 * 1_V * 1_s * 1_s / 1_rad; - static constexpr units::meter_t kTrackWidth = 1_m; - - const frc::LinearSystem<2, 2, 2> kDrivetrainPlant = - frc::LinearSystemId::IdentifyDrivetrainSystem(kv, ka, kvAngular, - kaAngular, kTrackWidth); - - frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ - kDrivetrainPlant, 2.0_ft, - frc::DCMotor::CIM(2), 8.0, - 6.0_in / 2, {0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}}; - - // Simulated Vision System. - // Configure these to match your PhotonVision Camera, - // pipeline, and LED setup. - units::degree_t camDiagFOV = 170.0_deg; // assume wide-angle camera - units::meter_t camHeightOffGround = 24_in; - units::meter_t maxLEDRange = 20_m; - int camResolutionWidth = 640; // pixels - int camResolutionHeight = 480; // pixels - double minTargetArea = 10; // square pixels - - photonlib::SimVisionSystem simVision{"photonvision", camDiagFOV, - frc::Transform2d{}, camHeightOffGround, - maxLEDRange, camResolutionWidth, - camResolutionHeight, minTargetArea}; - - // See - // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf - // page 208 - const units::meter_t targetWidth = 41.30_in - 6.70_in; - // See - // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/2020FieldDrawing-SeasonSpecific.pdf - // page 197 - const units::meter_t targetHeight = 98.19_in - 81.19_in; // meters - // See - // https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf - // pages 4 and 5 - const units::meter_t tgtXPos = 54_ft; - const units::meter_t tgtYPos = (27.0_ft / 2) - 43.75_in - (48.0_in / 2.0); - const frc::Translation2d targetTrans{tgtXPos, tgtYPos}; - const frc::Rotation2d targetRot{0.0_deg}; - frc::Pose2d farTargetPose{targetTrans, targetRot}; - - frc::Field2d field{}; -}; diff --git a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/Robot.h b/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/Robot.h deleted file mode 100644 index e4b21c700..000000000 --- a/photonlib-cpp-examples/src/main/cpp/examples/simaimandrange/include/Robot.h +++ /dev/null @@ -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 - -#include -#include -#include -#include -#include -#include -#include - -#include "DrivetrainSim.h" - -class Robot : public frc::TimedRobot { - public: - void TeleopPeriodic() override; - void SimulationPeriodic() override; - - private: - // Constants such as camera and target height stored. Change per robot and - // goal! - const units::meter_t CAMERA_HEIGHT = 24_in; - const units::meter_t TARGET_HEIGHT = 81.19_in; - - // Angle between horizontal and the camera. - const units::radian_t CAMERA_PITCH = 15_deg; - - // How far from the target we want to be - const units::meter_t GOAL_RANGE_METERS = 10_ft; - - // PID constants should be tuned per robot - const double LINEAR_P = 2.0; - const double LINEAR_D = 0.0; - frc2::PIDController forwardController{LINEAR_P, 0.0, LINEAR_D}; - - const double ANGULAR_P = 0.03; - const double ANGULAR_D = 0.003; - frc2::PIDController turnController{ANGULAR_P, 0.0, ANGULAR_D}; - - // Change this to match the name of your camera - photonlib::PhotonCamera camera{"photonvision"}; - - frc::XboxController xboxController{0}; - - // Drive motors - frc::PWMVictorSPX leftMotor{0}; - frc::PWMVictorSPX rightMotor{1}; - frc::DifferentialDrive drive{leftMotor, rightMotor}; - - // Simulation Support - DrivetrainSim dtSim{}; -}; diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/sim/DrivetrainSim.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/sim/DrivetrainSim.java index 63a741450..5fcd36bf1 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/sim/DrivetrainSim.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simaimandrange/sim/DrivetrainSim.java @@ -25,9 +25,10 @@ package org.photonlib.examples.simaimandrange.sim; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; @@ -73,7 +74,7 @@ public class DrivetrainSim { // Configure these to match your PhotonVision Camera, // pipeline, and LED setup. double camDiagFOV = 170.0; // degrees - assume wide-angle camera - double camPitch = Units.radiansToDegrees(Robot.CAMERA_PITCH_RADIANS); // degrees + double camPitch = Robot.CAMERA_PITCH_RADIANS; // degrees double camHeightOffGround = Robot.CAMERA_HEIGHT_METERS; // meters double maxLEDRange = 20; // meters int camResolutionWidth = 640; // pixels @@ -84,9 +85,8 @@ public class DrivetrainSim { new SimVisionSystem( "photonvision", camDiagFOV, - camPitch, - new Transform2d(), - camHeightOffGround, + new Transform3d( + new Translation3d(0, 0, camHeightOffGround), new Rotation3d(0, camPitch, 0)), maxLEDRange, camResolutionWidth, camResolutionHeight, @@ -105,13 +105,15 @@ public class DrivetrainSim { double tgtXPos = Units.feetToMeters(54); double tgtYPos = Units.feetToMeters(27 / 2) - Units.inchesToMeters(43.75) - Units.inchesToMeters(48.0 / 2.0); - Pose2d farTargetPose = new Pose2d(new Translation2d(tgtXPos, tgtYPos), new Rotation2d(0.0)); + Pose3d farTargetPose = + new Pose3d( + new Translation3d(tgtXPos, tgtYPos, Robot.TARGET_HEIGHT_METERS), + new Rotation3d(0.0, 0.0, 0.0)); Field2d field = new Field2d(); public DrivetrainSim() { - simVision.addSimVisionTarget( - new SimVisionTarget(farTargetPose, Robot.TARGET_HEIGHT_METERS, targetWidth, targetHeight)); + simVision.addSimVisionTarget(new SimVisionTarget(farTargetPose, targetWidth, targetHeight, -1)); SmartDashboard.putData("Field", field); } diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java index a13fa154b..916108632 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/Constants.java @@ -87,7 +87,6 @@ public class Constants { // page 197 public static final double targetHeight = Units.inchesToMeters(98.19) - Units.inchesToMeters(81.19); // meters - public static final double targetHeightAboveGround = Units.inchesToMeters(81.19); // meters // See https://firstfrc.blob.core.windows.net/frc2020/PlayingField/LayoutandMarkingDiagram.pdf // pages 4 and 5 @@ -103,6 +102,5 @@ public class Constants { new Rotation3d(0.0, 0.0, Units.degreesToRadians(180))); public static final SimVisionTarget kFarTarget = - new SimVisionTarget( - kFarTargetPose.toPose2d(), targetHeightAboveGround, targetWidth, targetHeight); + new SimVisionTarget(kFarTargetPose, targetWidth, targetHeight, 42); } diff --git a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java index 941c6a7eb..b5c5ac877 100644 --- a/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java +++ b/photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/sim/DrivetrainSim.java @@ -89,11 +89,7 @@ public class DrivetrainSim { new SimVisionSystem( Constants.kCamName, camDiagFOV, - camPitch, - new Transform2d( - Constants.kCameraToRobot.getTranslation().toTranslation2d(), - Constants.kCameraToRobot.getRotation().toRotation2d()), - camHeightOffGround, + Constants.kCameraToRobot, maxLEDRange, camResolutionWidth, camResolutionHeight,