diff --git a/photon-lib/src/main/native/include/photonlib/Packet.h b/photon-lib/src/main/native/include/photonlib/Packet.h index 43596ab6b8..a74b8f69c3 100644 --- a/photon-lib/src/main/native/include/photonlib/Packet.h +++ b/photon-lib/src/main/native/include/photonlib/Packet.h @@ -99,13 +99,15 @@ class Packet { */ template Packet& operator>>(T& value) { - std::memcpy(&value, packetData.data() + readPos, sizeof(T)); - - if constexpr (wpi::support::endian::system_endianness() == - wpi::support::endianness::little) { - // Reverse to little endian for host. - char& raw = reinterpret_cast(value); - std::reverse(&raw, &raw + sizeof(T)); + if (!packetData.empty()) { + std::memcpy(&value, packetData.data() + readPos, sizeof(T)); + + if constexpr (wpi::support::endian::system_endianness() == + wpi::support::endianness::little) { + // Reverse to little endian for host. + char& raw = reinterpret_cast(value); + std::reverse(&raw, &raw + sizeof(T)); + } } readPos += sizeof(T); diff --git a/photon-lib/src/main/native/include/photonlib/PhotonTargetSortMode.h b/photon-lib/src/main/native/include/photonlib/PhotonTargetSortMode.h new file mode 100644 index 0000000000..8f650215f6 --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/PhotonTargetSortMode.h @@ -0,0 +1,85 @@ +/* + * 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 "photonlib/PhotonTrackedTarget.h" + +namespace photonlib { + +namespace PhotonTargetSortMode { + +struct Smallest { + inline bool operator()(const PhotonTrackedTarget& target1, + const PhotonTrackedTarget& target2) { + return target1.GetArea() < target2.GetArea(); + } +}; + +struct Largest { + inline bool operator()(const PhotonTrackedTarget& target1, + const PhotonTrackedTarget& target2) { + return target1.GetArea() > target2.GetArea(); + } +}; + +struct Highest { + inline bool operator()(const PhotonTrackedTarget& target1, + const PhotonTrackedTarget& target2) { + return target1.GetPitch() < target2.GetPitch(); + } +}; + +struct Lowest { + inline bool operator()(const PhotonTrackedTarget& target1, + const PhotonTrackedTarget& target2) { + return target1.GetPitch() > target2.GetPitch(); + } +}; + +struct RightMost { + inline bool operator()(const PhotonTrackedTarget& target1, + const PhotonTrackedTarget& target2) { + return target1.GetYaw() < target2.GetYaw(); + } +}; + +struct LeftMost { + inline bool operator()(const PhotonTrackedTarget& target1, + const PhotonTrackedTarget& target2) { + return target1.GetYaw() > target2.GetYaw(); + } +}; + +struct CenterMost { + inline bool operator()(const PhotonTrackedTarget& target1, + const PhotonTrackedTarget& target2) { + return std::pow(target1.GetPitch(), 2) + std::pow(target1.GetYaw(), 2) < + std::pow(target2.GetPitch(), 2) + std::pow(target2.GetYaw(), 2); + } +}; +} // namespace PhotonTargetSortMode +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h new file mode 100644 index 0000000000..275912ea0c --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h @@ -0,0 +1,126 @@ +/* + * 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/PhotonCamera.h" +#include "photonlib/PhotonTargetSortMode.h" + +namespace photonlib { +class SimPhotonCamera : public PhotonCamera { + public: + SimPhotonCamera(std::shared_ptr instance, + const std::string& cameraName) + : PhotonCamera(instance, cameraName) { + latencyMillisEntry = rootTable->GetEntry("latencyMillis"); + hasTargetEntry = rootTable->GetEntry("hasTargetEntry"); + targetPitchEntry = rootTable->GetEntry("targetPitchEntry"); + targetYawEntry = rootTable->GetEntry("targetYawEntry"); + targetAreaEntry = rootTable->GetEntry("targetAreaEntry"); + targetSkewEntry = rootTable->GetEntry("targetSkewEntry"); + targetPoseEntry = rootTable->GetEntry("targetPoseEntry"); + versionEntry = instance->GetTable("photonvision")->GetEntry("version"); + // versionEntry.SetString(PhotonVersion.versionString); + } + + explicit SimPhotonCamera(const std::string& cameraName) + : SimPhotonCamera(std::make_shared( + nt::NetworkTableInstance::GetDefault()), + cameraName) {} + + /** + * Simulate one processed frame of vision data, putting one result to NT. + * + * @param latency Latency of the provided frame + * @param targetList List of targets detected + */ + void SubmitProcessedFrame(units::millisecond_t latency, + std::vector targetList) { + SubmitProcessedFrame(latency, PhotonTargetSortMode::LeftMost(), targetList); + } + + /** + * Simulate one processed frame of vision data, putting one result to NT. + * + * @param latency Latency of the provided frame + * @param sortMode Order in which to sort targets + * @param targetList List of targets detected + */ + void SubmitProcessedFrame( + units::millisecond_t latency, + std::function + sortMode, + std::vector targetList) { + latencyMillisEntry.SetDouble(latency.to()); + std::sort(targetList.begin(), targetList.end(), + [&](auto lhs, auto rhs) { return sortMode(lhs, rhs); }); + PhotonPipelineResult newResult{latency, targetList}; + Packet packet{}; + packet << newResult; + rawBytesEntry.SetRaw( + std::string_view{packet.GetData().data(), packet.GetDataSize()}); + + std::string rawBytesGet = rawBytesEntry.GetRaw("ohono"); + + bool hasTargets = newResult.HasTargets(); + hasTargetEntry.SetBoolean(hasTargets); + if (!hasTargets) { + targetPitchEntry.SetDouble(0.0); + targetYawEntry.SetDouble(0.0); + targetAreaEntry.SetDouble(0.0); + targetPoseEntry.SetDoubleArray({0.0, 0.0, 0.0}); + targetSkewEntry.SetDouble(0.0); + } else { + PhotonTrackedTarget bestTarget = newResult.GetBestTarget(); + targetPitchEntry.SetDouble(bestTarget.GetPitch()); + targetYawEntry.SetDouble(bestTarget.GetYaw()); + targetAreaEntry.SetDouble(bestTarget.GetArea()); + targetSkewEntry.SetDouble(bestTarget.GetSkew()); + + frc::Transform3d transform = bestTarget.GetBestCameraToTarget(); + targetPoseEntry.SetDoubleArray( + {transform.X().to(), transform.Y().to(), + transform.Rotation().ToRotation2d().Degrees().to()}); + } + } + + private: + nt::NetworkTableEntry latencyMillisEntry; + nt::NetworkTableEntry hasTargetEntry; + nt::NetworkTableEntry targetPitchEntry; + nt::NetworkTableEntry targetYawEntry; + nt::NetworkTableEntry targetAreaEntry; + nt::NetworkTableEntry targetSkewEntry; + nt::NetworkTableEntry targetPoseEntry; + nt::NetworkTableEntry versionEntry; +}; +} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h b/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h new file mode 100644 index 0000000000..962ea7a394 --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/SimVisionSystem.h @@ -0,0 +1,208 @@ +/* + * 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 "SimPhotonCamera.h" +#include "SimVisionTarget.h" + +namespace photonlib { +class SimVisionSystem { + public: + SimPhotonCamera cam; + units::radian_t camHorizFOV{0}; + units::radian_t camVertFOV{0}; + units::meter_t maxLEDRange{0}; + int cameraResWidth{0}; + int cameraResHeight{0}; + double minTargetArea{0.0}; + frc::Transform3d cameraToRobot; + + frc::Field2d dbgField; + frc::FieldObject2d* dbgRobot; + frc::FieldObject2d* dbgCamera; + + std::vector targetList; + + /** + * Create a simulated vision system involving a camera and coprocessor mounted + * on a mobile robot running PhotonVision, detecting one or more targets + * scattered around the field. This assumes a fairly simple and + * distortion-less pinhole camera model. + * + * @param camName Name of the PhotonVision camera to create. Align it with the + * settings you use in the PhotonVision GUI. + * @param camDiagFOV 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 cameraToRobot Transform to move from the camera's mount position to + * the robot's position + * @param maxLEDRange Maximum distance at which your camera can illuminate the + * target and make it visible. Set to 9000 or more if your vision system does + * not rely on LED's. + * @param cameraResWidth Width of your camera's image sensor in pixels + * @param cameraResHeight Height of your camera's image sensor in pixels + * @param minTargetArea Minimum area that that the target should be before + * it's recognized as a target by the camera. Match this with your contour + * filtering settings in the PhotonVision GUI. + */ + SimVisionSystem(std::string camName, units::degree_t camDiagFOV, + frc::Transform3d cameraToRobot, units::meter_t maxLEDRange, + int cameraResWidth, int cameraResHeight, double minTargetArea) + : cam(camName), + camHorizFOV((camDiagFOV * cameraResWidth) / + std::hypot(cameraResWidth, cameraResHeight)), + camVertFOV((camDiagFOV * cameraResHeight) / + std::hypot(cameraResWidth, cameraResHeight)), + maxLEDRange(maxLEDRange), + cameraResWidth(cameraResWidth), + cameraResHeight(cameraResHeight), + minTargetArea(minTargetArea), + cameraToRobot(cameraToRobot), + dbgField(), + dbgRobot(dbgField.GetRobotObject()), + dbgCamera(dbgField.GetObject(camName + " Camera")) { + frc::SmartDashboard::PutData(camName + " Sim Field", &dbgField); + } + + /** + * Add a target on the field which your vision system is designed to detect. + * The PhotonCamera from this system will report the location of the robot + * relative to the subset of these targets which are visible from the given + * robot position. + * + * @param target Target to add to the simulated field + */ + void AddSimVisionTarget(SimVisionTarget target) { + targetList.push_back(target); + dbgField.GetObject("Target " + std::to_string(target.targetId)) + ->SetPose(target.targetPose.ToPose2d()); + } + + /** + * Adjust the camera position relative to the robot. Use this if your camera + * is on a gimbal or turret or some other mobile platform. + * + * @param newCameraToRobot New Transform from the robot to the camera + */ + void MoveCamera(frc::Transform3d newCameraToRobot) { + cameraToRobot = newCameraToRobot; + } + + /** + * Periodic update. Call this once per frame of image data you wish to process + * and send to NetworkTables + * + * @param robotPose current pose of the robot on the field. Will be used to + * calculate which targets are actually in view, where they are at relative to + * the robot, and relevant PhotonVision parameters. + */ + void ProcessFrame(frc::Pose2d robotPose) { + ProcessFrame(frc::Pose3d{ + robotPose.X(), robotPose.Y(), 0.0_m, + frc::Rotation3d{0_rad, 0_rad, robotPose.Rotation().Radians()}}); + } + + /** + * Periodic update. Call this once per frame of image data you wish to process + * and send to NetworkTables + * + * @param robotPose 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. + */ + void ProcessFrame(frc::Pose3d robotPose) { + frc::Pose3d cameraPose = robotPose.TransformBy(cameraToRobot.Inverse()); + dbgRobot->SetPose(robotPose.ToPose2d()); + dbgCamera->SetPose(cameraPose.ToPose2d()); + + std::vector visibleTargetList{}; + + for (const auto& target : targetList) { + frc::Transform3d camToTargetTransform{cameraPose, target.targetPose}; + frc::Translation3d camToTargetTranslation{ + camToTargetTransform.Translation()}; + + frc::Translation3d altTranslation{camToTargetTranslation.X(), + -1.0 * camToTargetTranslation.Y(), + camToTargetTranslation.Z()}; + frc::Rotation3d altRotation{camToTargetTransform.Rotation() * -1.0}; + frc::Transform3d camToTargetAltTransform{altTranslation, altRotation}; + units::meter_t dist{camToTargetTranslation.Norm()}; + double areaPixels{target.targetArea / GetM2PerPx(dist)}; + units::radian_t yaw{units::math::atan2(camToTargetTranslation.Y(), + camToTargetTranslation.X())}; + units::meter_t cameraHeightOffGround{cameraPose.Z()}; + units::meter_t targetHeightAboveGround(target.targetPose.Z()); + units::radian_t camPitch{cameraPose.Rotation().Y()}; + frc::Transform2d transformAlongGround{cameraPose.ToPose2d(), + target.targetPose.ToPose2d()}; + units::meter_t distanceAlongGround{ + transformAlongGround.Translation().Norm()}; + units::radian_t pitch = + units::math::atan2(targetHeightAboveGround - cameraHeightOffGround, + distanceAlongGround) - + camPitch; + + if (CamCamSeeTarget(dist, yaw, pitch, areaPixels)) { + visibleTargetList.push_back( + PhotonTrackedTarget{yaw.convert().to(), + pitch.convert().to(), + areaPixels, + 0.0, + target.targetId, + camToTargetTransform, + {{0, 0}, {0, 0}, {0, 0}, {0, 0}}}); + } + + cam.SubmitProcessedFrame(0_s, visibleTargetList); + } + } + + units::square_meter_t GetM2PerPx(units::meter_t dist) { + units::meter_t widthMPerPx = + 2 * dist * units::math::tan(camHorizFOV / 2) / cameraResWidth; + units::meter_t heightMPerPx = + 2 * dist * units::math::tan(camVertFOV / 2) / cameraResHeight; + return widthMPerPx * heightMPerPx; + } + + bool CamCamSeeTarget(units::meter_t dist, units::radian_t yaw, + units::radian_t pitch, double area) { + bool inRange = dist < 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/include/photonlib/SimVisionTarget.h b/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h new file mode 100644 index 0000000000..39fe377b7a --- /dev/null +++ b/photon-lib/src/main/native/include/photonlib/SimVisionTarget.h @@ -0,0 +1,59 @@ +/* + * 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 + +namespace photonlib { +class SimVisionTarget { + public: + SimVisionTarget() = default; + + /** + * Describes a vision target located somewhere on the field that your + * SimVisionSystem can detect. + * + * @param targetPose Pose3d of the target in field-relative coordinates + * @param targetWidth Width of the outer bounding box of the target. + * @param targetHeight Pair Height of the outer bounding box of the + * target. + * @param targetId Id of the target + */ + SimVisionTarget(frc::Pose3d targetPose, units::meter_t targetWidth, + units::meter_t targetHeight, int targetId) + : targetPose(targetPose), + targetWidth(targetWidth), + targetHeight(targetHeight), + targetArea(targetHeight * targetWidth), + targetId(targetId) {} + + frc::Pose3d targetPose; + units::meter_t targetWidth; + units::meter_t targetHeight; + units::square_meter_t targetArea; + int targetId; +}; +} // namespace photonlib diff --git a/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp b/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp new file mode 100644 index 0000000000..6a70b70207 --- /dev/null +++ b/photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp @@ -0,0 +1,362 @@ +/* + * 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 "gtest/gtest.h" +#include "photonlib/PhotonUtils.h" +#include "photonlib/SimVisionSystem.h" + +class SimVisionSystemTest : public ::testing::Test { + void SetUp() override { + nt::NetworkTableInstance::GetDefault().StartServer(); + photonlib::PhotonCamera::SetVersionCheckEnabled(false); + } + + void TearDown() override {} +}; + +class SimVisionSystemTestWithParamsTest + : public SimVisionSystemTest, + public testing::WithParamInterface {}; +class SimVisionSystemTestDistanceParamsTest + : public SimVisionSystemTest, + public testing::WithParamInterface< + std::tuple> {}; + +TEST_F(SimVisionSystemTest, TestEmpty) { + photonlib::SimVisionSystem sys{ + "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 320, 240, 0}; + SUCCEED(); +} + +TEST_F(SimVisionSystemTest, TestVisibilityCupidShuffle) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; + photonlib::SimVisionSystem sys{ + "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0}; + sys.AddSimVisionTarget(photonlib::SimVisionTarget{targetPose, 1_m, 3_m, 3}); + + // To the right, to the right + frc::Pose2d robotPose{{5_m, 0_m}, frc::Rotation2d{-70_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets()); + + // To the right, to the right + robotPose = frc::Pose2d{{5_m, 0_m}, frc::Rotation2d{-95_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = frc::Pose2d{{5_m, 0_m}, frc::Rotation2d{90_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = frc::Pose2d{{5_m, 0_m}, frc::Rotation2d{65_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets()); + + // now kick, now kick + robotPose = frc::Pose2d{{2_m, 0_m}, frc::Rotation2d{5_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets()); + + // now kick, now kick + robotPose = frc::Pose2d{{2_m, 0_m}, frc::Rotation2d{-5_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets()); + + // now walk it by yourself + robotPose = frc::Pose2d{{2_m, 0_m}, frc::Rotation2d{-179_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets()); + + // now walk it by yourself + sys.MoveCamera(frc::Transform3d{ + frc::Translation3d{}, + frc::Rotation3d{0_rad, 0_rad, units::constants::detail::PI_VAL * 1_rad}}); + sys.ProcessFrame(robotPose); + ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets()); +} + +TEST_F(SimVisionSystemTest, TestNotVisibleVertOne) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; + photonlib::SimVisionSystem sys{ + "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0}; + sys.AddSimVisionTarget(photonlib::SimVisionTarget{targetPose, 1_m, 3_m, 3}); + + frc::Pose2d robotPose{{5_m, 0_m}, frc::Rotation2d{5_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets()); + + sys.MoveCamera(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 5000_m}, + frc::Rotation3d{0_rad, 0_rad, units::constants::detail::PI_VAL * 1_rad}}); + sys.ProcessFrame(robotPose); + ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets()); +} + +TEST_F(SimVisionSystemTest, TestNotVisibleVertTwo) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; + frc::Transform3d robotToCamera{ + frc::Translation3d{0_m, 0_m, 1_m}, + frc::Rotation3d{0_deg, (units::constants::detail::PI_VAL / 4) * 1_rad, + 0_deg}}; + photonlib::SimVisionSystem sys{ + "Test", 80.0_deg, robotToCamera.Inverse(), 99999_m, 1234, 1234, 0}; + sys.AddSimVisionTarget( + photonlib::SimVisionTarget{targetPose, 3_m, 0.5_m, 1736}); + + frc::Pose2d robotPose{{14.98_m, 0_m}, frc::Rotation2d{5_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets()); +} + +TEST_F(SimVisionSystemTest, TestNotVisibleTargetSize) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; + photonlib::SimVisionSystem sys{ + "Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 20.0}; + sys.AddSimVisionTarget( + photonlib::SimVisionTarget{targetPose, 0.1_m, 0.025_m, 24}); + + frc::Pose2d robotPose{{12_m, 0_m}, frc::Rotation2d{5_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets()); +} + +TEST_F(SimVisionSystemTest, TestNotVisibleTooFarForLEDs) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; + photonlib::SimVisionSystem sys{ + "Test", 80.0_deg, frc::Transform3d{}, 10_m, 640, 480, 1.0}; + sys.AddSimVisionTarget( + photonlib::SimVisionTarget{targetPose, 1_m, 0.25_m, 78}); + + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{5_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + sys.ProcessFrame(robotPose); + ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets()); +} + +TEST_P(SimVisionSystemTestWithParamsTest, YawAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, + (3 * units::constants::detail::PI_VAL / 4) * 1_rad}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + photonlib::SimVisionSystem sys{ + "Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0}; + sys.AddSimVisionTarget( + photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23}); + + sys.ProcessFrame(robotPose); + + auto results = sys.cam.GetLatestResult(); + + ASSERT_TRUE(results.HasTargets()); + + photonlib::PhotonTrackedTarget target = results.GetBestTarget(); + + ASSERT_NEAR(GetParam().to(), target.GetYaw(), 0.0001); +} + +TEST_P(SimVisionSystemTestWithParamsTest, PitchAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, + (3 * units::constants::detail::PI_VAL / 4) * 1_rad}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{0_deg}}; + photonlib::SimVisionSystem sys{ + "Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0}; + sys.AddSimVisionTarget( + photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23}); + + sys.MoveCamera(frc::Transform3d{frc::Translation3d{}, + frc::Rotation3d{0_deg, GetParam(), 0_deg}}); + sys.ProcessFrame(robotPose); + + auto results = sys.cam.GetLatestResult(); + + ASSERT_TRUE(results.HasTargets()); + + photonlib::PhotonTrackedTarget target = results.GetBestTarget(); + + ASSERT_NEAR(GetParam().to(), target.GetPitch(), 0.0001); +} + +INSTANTIATE_TEST_SUITE_P(AnglesTests, SimVisionSystemTestWithParamsTest, + testing::Values(-10_deg, -5_deg, -0_deg, -1_deg, + -2_deg, 5_deg, 7_deg, 10.23_deg, + 20.21_deg, -19.999_deg)); + +TEST_P(SimVisionSystemTestDistanceParamsTest, DistanceCalc) { + units::foot_t distParam; + units::degree_t pitchParam; + units::foot_t heightParam; + std::tie(distParam, pitchParam, heightParam) = GetParam(); + + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_deg, 0_deg, + (units::constants::detail::PI_VAL * 0.98) * 1_rad}}; + frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}}; + frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam}, + frc::Rotation3d{0_deg, pitchParam, 0_deg}}; + photonlib::SimVisionSystem sys{ + "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho" + "wsyourdaygoingihopegoodhaveagreatrestofyourlife", + 160.0_deg, + robotToCamera.Inverse(), + 99999_m, + 640, + 480, + 0}; + sys.AddSimVisionTarget( + photonlib::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 0}); + sys.ProcessFrame(robotPose); + + auto results = sys.cam.GetLatestResult(); + + ASSERT_TRUE(results.HasTargets()); + + photonlib::PhotonTrackedTarget target = results.GetBestTarget(); + + ASSERT_NEAR(target.GetYaw(), 0.0, 0.0001); + + units::meter_t dist = photonlib::PhotonUtils::CalculateDistanceToTarget( + robotToCamera.Z(), targetPose.Z(), pitchParam, + units::degree_t{target.GetPitch()}); + ASSERT_NEAR(dist.to(), + distParam.convert().to(), 0.001); +} + +INSTANTIATE_TEST_SUITE_P( + DistanceParamsTests, SimVisionSystemTestDistanceParamsTest, + testing::Values(std::make_tuple(5_ft, 15.98_deg, 0_ft), + std::make_tuple(6_ft, 15.98_deg, 1_ft), + std::make_tuple(10_ft, 15.98_deg, 0_ft), + std::make_tuple(15_ft, 15.98_deg, 2_ft), + std::make_tuple(19.95_ft, 15.98_deg, 0_ft), + std::make_tuple(20_ft, 15.98_deg, 0_ft), + std::make_tuple(5_ft, 42_deg, 1_ft), + std::make_tuple(6_ft, 42_deg, 0_ft), + std::make_tuple(10_ft, 42_deg, 2_ft), + std::make_tuple(15_ft, 42_deg, 0.5_ft), + std::make_tuple(19.42_ft, 15.98_deg, 0_ft), + std::make_tuple(20_ft, 42_deg, 0_ft), + std::make_tuple(5_ft, 55_deg, 2_ft), + std::make_tuple(6_ft, 55_deg, 0_ft), + std::make_tuple(10_ft, 54_deg, 2.2_ft), + std::make_tuple(15_ft, 53_deg, 0_ft), + std::make_tuple(19.52_ft, 15.98_deg, 1.1_ft))); + +TEST_F(SimVisionSystemTest, TestMultipleTargets) { + const frc::Pose3d targetPoseL{ + {15.98_m, 2_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; + const frc::Pose3d targetPoseC{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; + const frc::Pose3d targetPoseR{ + {15.98_m, -2_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}}; + photonlib::SimVisionSystem sys{ + "Test", 160.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 20.0}; + sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + 0.25_m, 0.25_m, 1}); + sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + 0.25_m, 0.25_m, 2}); + sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + 0.25_m, 0.25_m, 3}); + + sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + 0.25_m, 0.25_m, 4}); + sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + 0.25_m, 0.25_m, 5}); + sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + 0.25_m, 0.25_m, 6}); + + sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + 0.25_m, 0.25_m, 7}); + sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + 0.25_m, 0.25_m, 8}); + sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + 0.25_m, 0.25_m, 9}); + sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + 0.25_m, 0.25_m, 10}); + sys.AddSimVisionTarget(photonlib::SimVisionTarget{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}), + 0.25_m, 0.25_m, 11}); + + frc::Pose2d robotPose{{6_m, 0_m}, frc::Rotation2d{.25_deg}}; + sys.ProcessFrame(robotPose); + + auto results = sys.cam.GetLatestResult(); + + ASSERT_TRUE(results.HasTargets()); + + wpi::span targetList = + results.GetTargets(); + + ASSERT_EQ(targetList.size(), size_t(11)); +} diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java index 8d9194ebe7..9464dabf13 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java @@ -129,6 +129,9 @@ public void encode(boolean src) { * @return A decoded byte from the packet. */ public byte decodeByte() { + if (packetData.length < readPos) { + return '\0'; + } return packetData[readPos++]; } @@ -138,6 +141,9 @@ public byte decodeByte() { * @return A decoded int from the packet. */ public int decodeInt() { + if (packetData.length < readPos + 3) { + return 0; + } return (0xff & packetData[readPos++]) << 24 | (0xff & packetData[readPos++]) << 16 | (0xff & packetData[readPos++]) << 8 @@ -150,6 +156,9 @@ public int decodeInt() { * @return A decoded double from the packet. */ public double decodeDouble() { + if (packetData.length < (readPos + 7)) { + return 0; + } long data = (long) (0xff & packetData[readPos++]) << 56 | (long) (0xff & packetData[readPos++]) << 48 @@ -168,6 +177,9 @@ public double decodeDouble() { * @return A decoded boolean from the packet. */ public boolean decodeBoolean() { + if (packetData.length < readPos) { + return false; + } return packetData[readPos++] == 1; } }