diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 95eecb444c..ff26f74fb8 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include "PhotonVersion.h" @@ -128,8 +129,13 @@ LEDMode PhotonCamera::GetLEDMode() const { std::optional PhotonCamera::GetCameraMatrix() { auto camCoeffs = cameraIntrinsicsSubscriber.Get(); if (camCoeffs.size() == 9) { - // clone should deal with ownership concerns? not sure - return cv::Mat(3, 3, CV_64FC1, camCoeffs.data()).clone(); + cv::Mat retVal(3, 3, CV_64FC1); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + retVal.at(i, j) = camCoeffs[(j * 3) + i]; + } + } + return retVal; } return std::nullopt; } @@ -145,8 +151,11 @@ const std::string_view PhotonCamera::GetCameraName() const { std::optional PhotonCamera::GetDistCoeffs() { auto distCoeffs = cameraDistortionSubscriber.Get(); if (distCoeffs.size() == 5) { - // clone should deal with ownership concerns? not sure - return cv::Mat(5, 1, CV_64FC1, distCoeffs.data()).clone(); + cv::Mat retVal(5, 1, CV_64FC1); + for (int i = 0; i < 5; i++) { + retVal.at(i, 0) = distCoeffs[i]; + } + return retVal; } return std::nullopt; } diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 2a1418f1b5..582458e014 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -39,11 +39,7 @@ #include #include -#include "photon/dataflow/structures/Packet.h" -#include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" -#include "photon/targeting/PhotonPipelineResult.h" -#include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting//PhotonPipelineResult.h" namespace cv { class Mat; @@ -172,6 +168,8 @@ class PhotonCamera { PhotonCamera::VERSION_CHECK_ENABLED = enabled; } + std::shared_ptr GetCameraTable() const { return rootTable; } + // For use in tests bool test = false; PhotonPipelineResult testResult; diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h new file mode 100644 index 0000000000..367b30bb8f --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -0,0 +1,449 @@ +/* + * MIT License + * + * Copyright (c) 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 + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace photon { +class PhotonCameraSim { + public: + explicit PhotonCameraSim(PhotonCamera* camera) + : PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG()) {} + PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props) + : prop(props), cam(camera) { + SetMinTargetAreaPixels(kDefaultMinAreaPx); + videoSimRaw = frc::CameraServer::PutVideo( + std::string{camera->GetCameraName()} + "-raw", prop.GetResWidth(), + prop.GetResHeight()); + videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray); + videoSimProcessed = frc::CameraServer::PutVideo( + std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(), + prop.GetResHeight()); + ts.subTable = cam->GetCameraTable(); + ts.UpdateEntries(); + } + PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props, + double minTargetAreaPercent, units::meter_t maxSightRange) + : PhotonCameraSim(camera, props) { + this->minTargetAreaPercent = minTargetAreaPercent; + this->maxSightRange = maxSightRange; + } + PhotonCamera* GetCamera() { return cam; } + double GetMinTargetAreaPercent() { return minTargetAreaPercent; } + double GetMinTargetAreaPixels() { + return minTargetAreaPercent / 100.0 * prop.GetResArea(); + } + units::meter_t GetMaxSightRange() { return maxSightRange; } + const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; } + const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; } + bool CanSeeTargetPose(const frc::Pose3d& camPose, + const VisionTargetSim& target) { + CameraTargetRelation rel{camPose, target.GetPose()}; + return ((units::math::abs(rel.camToTargYaw.Degrees()) < + prop.GetHorizFOV().Degrees() / 2.0) && + (units::math::abs(rel.camToTargPitch.Degrees()) < + prop.GetVertFOV().Degrees() / 2.0) && + (!target.GetModel().GetIsPlanar() || + units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) && + (rel.camToTarg.Translation().Norm() <= maxSightRange)); + } + bool CanSeeCorner(const std::vector& points) { + for (const auto& pt : points) { + if (std::clamp(pt.x, 0, prop.GetResWidth()) != pt.x || + std::clamp(pt.y, 0, prop.GetResHeight()) != pt.y) { + return false; + } + } + return true; + } + std::optional ConsumeNextEntryTime() { + uint64_t now = wpi::Now(); + uint64_t timestamp{}; + int iter = 0; + while (now >= nextNTEntryTime) { + timestamp = nextNTEntryTime; + uint64_t frameTime = prop.EstSecUntilNextFrame() + .convert() + .to(); + nextNTEntryTime += frameTime; + + if (iter++ > 50) { + timestamp = now; + nextNTEntryTime = now + frameTime; + break; + } + } + + if (timestamp != 0) { + return timestamp; + } else { + return std::nullopt; + } + } + void SetMinTargetAreaPercent(double areaPercent) { + minTargetAreaPercent = areaPercent; + } + void SetMinTargetAreaPixels(double areaPx) { + minTargetAreaPercent = areaPx / prop.GetResArea() * 100; + } + void SetMaxSightRange(units::meter_t range) { maxSightRange = range; } + void EnableRawStream(bool enabled) { videoSimRawEnabled = enabled; } + void EnableDrawWireframe(bool enabled) { videoSimWireframeEnabled = enabled; } + void SetWireframeResolution(double resolution) { + videoSimWireframeResolution = resolution; + } + void EnabledProcessedStream(double enabled) { videoSimProcEnabled = enabled; } + PhotonPipelineResult Process(units::second_t latency, + const frc::Pose3d& cameraPose, + std::vector targets) { + std::sort( + targets.begin(), targets.end(), + [cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) { + units::meter_t dist1 = + t1.GetPose().Translation().Distance(cameraPose.Translation()); + units::meter_t dist2 = + t2.GetPose().Translation().Distance(cameraPose.Translation()); + return dist1 > dist2; + }); + + std::vector>> + visibleTgts{}; + std::vector detectableTgts{}; + RotTrlTransform3d camRt = RotTrlTransform3d::MakeRelativeTo(cameraPose); + + VideoSimUtil::UpdateVideoProp(videoSimRaw, prop); + VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop); + cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()}; + cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1); + blankFrame.assignTo(videoSimFrameRaw); + + for (const auto& tgt : targets) { + if (!CanSeeTargetPose(cameraPose, tgt)) { + continue; + } + + std::vector fieldCorners = tgt.GetFieldVertices(); + if (tgt.GetModel().GetIsSpherical()) { + TargetModel model = tgt.GetModel(); + fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose( + tgt.GetPose().Translation(), cameraPose.Translation())); + } + + std::vector imagePoints = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, fieldCorners); + if (tgt.GetModel().GetIsSpherical()) { + cv::Point2d center = OpenCVHelp::AvgPoint(imagePoints); + int l = 0; + int t = 0; + int b = 0; + int r = 0; + for (int i = 0; i < 4; i++) { + if (imagePoints[i].x < imagePoints[l].x) { + l = i; + } + } + cv::Point2d lc = imagePoints[l]; + std::array angles{}; + t = (l + 1) % 4; + b = (l + 1) % 4; + for (int i = 0; i < 4; i++) { + if (i == l) { + continue; + } + cv::Point2d ic = imagePoints[i]; + angles[i] = std::atan2(lc.y - ic.y, ic.x - lc.x); + if (angles[i] >= angles[t]) { + t = i; + } + if (angles[i] <= angles[b]) { + b = i; + } + } + for (int i = 0; i < 4; i++) { + if (i != t && i != l && i != b) { + r = i; + } + } + cv::RotatedRect rect{ + cv::Point2d{center.x, center.y}, + cv::Size2d{imagePoints[r].x - lc.x, + imagePoints[b].y - imagePoints[t].y}, + units::radian_t{-angles[r]}.convert().to()}; + std::vector points{}; + rect.points(points); + + // Can't find an easier way to convert from Point2f to Point2d + imagePoints.clear(); + std::transform(points.begin(), points.end(), + std::back_inserter(imagePoints), + [](const cv::Point2f& p) { return (cv::Point2d)p; }); + } + + visibleTgts.emplace_back(std::make_pair(tgt, imagePoints)); + std::vector noisyTargetCorners = + prop.EstPixelNoise(imagePoints); + cv::RotatedRect minAreaRect = + OpenCVHelp::GetMinAreaRect(noisyTargetCorners); + std::vector minAreaRectPts; + minAreaRectPts.reserve(4); + minAreaRect.points(minAreaRectPts); + cv::Point2d centerPt = minAreaRect.center; + frc::Rotation3d centerRot = prop.GetPixelRot(centerPt); + double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners); + + if (!(CanSeeCorner(noisyTargetCorners) && + areaPercent >= minTargetAreaPercent)) { + continue; + } + + PNPResult pnpSim{}; + if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) { + pnpSim = OpenCVHelp::SolvePNP_Square( + prop.GetIntrinsics(), prop.GetDistCoeffs(), + tgt.GetModel().GetVertices(), noisyTargetCorners); + } + + std::vector> tempCorners = + OpenCVHelp::PointsToCorners(minAreaRectPts); + wpi::SmallVector, 4> smallVec; + + for (const auto& corner : tempCorners) { + smallVec.emplace_back( + std::make_pair(static_cast(corner.first), + static_cast(corner.second))); + } + + std::vector> cornersFloat = + OpenCVHelp::PointsToCorners(noisyTargetCorners); + + std::vector> cornersDouble{cornersFloat.begin(), + cornersFloat.end()}; + detectableTgts.emplace_back(PhotonTrackedTarget{ + centerRot.Z().convert().to(), + -centerRot.Y().convert().to(), areaPercent, + centerRot.X().convert().to(), tgt.fiducialId, + pnpSim.best, pnpSim.alt, pnpSim.ambiguity, smallVec, cornersDouble}); + } + + if (videoSimRawEnabled) { + if (videoSimWireframeEnabled) { + VideoSimUtil::DrawFieldWireFrame( + camRt, prop, videoSimWireframeResolution, 1.5, cv::Scalar{80}, 6, 1, + cv::Scalar{30}, videoSimFrameRaw); + } + + for (const auto& pair : visibleTgts) { + VisionTargetSim tgt = pair.first; + std::vector corners = pair.second; + + if (tgt.fiducialId > 0) { + VideoSimUtil::Warp165h5TagImage(tgt.fiducialId, corners, true, + videoSimFrameRaw); + } else if (!tgt.GetModel().GetIsSpherical()) { + std::vector contour = corners; + if (!tgt.GetModel().GetIsPlanar()) { + contour = OpenCVHelp::GetConvexHull(contour); + } + VideoSimUtil::DrawPoly(contour, -1, cv::Scalar{255}, true, + videoSimFrameRaw); + } else { + VideoSimUtil::DrawInscribedEllipse(corners, cv::Scalar{255}, + videoSimFrameRaw); + } + } + videoSimRaw.PutFrame(videoSimFrameRaw); + } else { + videoSimRaw.SetConnectionStrategy( + cs::VideoSource::ConnectionStrategy::kConnectionForceClose); + } + + if (videoSimProcEnabled) { + cv::cvtColor(videoSimFrameRaw, videoSimFrameProcessed, + cv::COLOR_GRAY2BGR); + cv::drawMarker( + videoSimFrameProcessed, + cv::Point2d{prop.GetResWidth() / 2.0, prop.GetResHeight() / 2.0}, + cv::Scalar{0, 255, 0}, cv::MARKER_CROSS, + static_cast( + VideoSimUtil::GetScaledThickness(15, videoSimFrameProcessed)), + static_cast( + VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)), + cv::LINE_AA); + for (const auto& tgt : detectableTgts) { + auto detectedCornersDouble = tgt.GetDetectedCorners(); + std::vector> detectedCornerFloat{ + detectedCornersDouble.begin(), detectedCornersDouble.end()}; + if (tgt.GetFiducialId() >= 0) { + VideoSimUtil::DrawTagDetection( + tgt.GetFiducialId(), + OpenCVHelp::CornersToPoints(detectedCornerFloat), + videoSimFrameProcessed); + } else { + cv::rectangle(videoSimFrameProcessed, + OpenCVHelp::GetBoundingRect( + OpenCVHelp::CornersToPoints(detectedCornerFloat)), + cv::Scalar{0, 0, 255}, + static_cast(VideoSimUtil::GetScaledThickness( + 1, videoSimFrameProcessed)), + cv::LINE_AA); + + wpi::SmallVector, 4> smallVec = + tgt.GetMinAreaRectCorners(); + + std::vector> cornersCopy{}; + cornersCopy.reserve(4); + + for (const auto& corner : smallVec) { + cornersCopy.emplace_back( + std::make_pair(static_cast(corner.first), + static_cast(corner.second))); + } + + VideoSimUtil::DrawPoly( + OpenCVHelp::CornersToPoints(cornersCopy), + static_cast( + VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)), + cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed); + } + } + videoSimProcessed.PutFrame(videoSimFrameProcessed); + } else { + videoSimProcessed.SetConnectionStrategy( + cs::VideoSource::ConnectionStrategy::kConnectionForceClose); + } + + MultiTargetPNPResult multiTagResults{}; + + std::vector visibleLayoutTags = + VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout); + if (visibleLayoutTags.size() > 1) { + wpi::SmallVector usedIds{}; + std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(), + usedIds.begin(), + [](const frc::AprilTag& tag) { return tag.ID; }); + std::sort(usedIds.begin(), usedIds.end()); + PNPResult pnpResult = VisionEstimation::EstimateCamPosePNP( + prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout, + kAprilTag16h5); + multiTagResults = MultiTargetPNPResult{pnpResult, usedIds}; + } + + return PhotonPipelineResult{latency, detectableTgts, multiTagResults}; + } + void SubmitProcessedFrame(const PhotonPipelineResult& result) { + SubmitProcessedFrame(result, wpi::Now()); + } + void SubmitProcessedFrame(const PhotonPipelineResult& result, + uint64_t recieveTimestamp) { + ts.latencyMillisEntry.Set( + result.GetLatency().convert().to(), + recieveTimestamp); + + Packet newPacket{}; + newPacket << result; + ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp); + + bool hasTargets = result.HasTargets(); + ts.hasTargetEntry.Set(hasTargets, recieveTimestamp); + if (!hasTargets) { + ts.targetPitchEntry.Set(0.0, recieveTimestamp); + ts.targetYawEntry.Set(0.0, recieveTimestamp); + ts.targetAreaEntry.Set(0.0, recieveTimestamp); + std::array poseData{0.0, 0.0, 0.0}; + ts.targetPoseEntry.Set(poseData, recieveTimestamp); + ts.targetSkewEntry.Set(0.0, recieveTimestamp); + } else { + PhotonTrackedTarget bestTarget = result.GetBestTarget(); + + ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp); + ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp); + ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp); + ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp); + + frc::Transform3d transform = bestTarget.GetBestCameraToTarget(); + std::array poseData{ + transform.X().to(), transform.Y().to(), + transform.Rotation().ToRotation2d().Degrees().to()}; + ts.targetPoseEntry.Set(poseData, recieveTimestamp); + } + + auto intrinsics = prop.GetIntrinsics(); + std::vector intrinsicsView{intrinsics.data(), + intrinsics.data() + intrinsics.size()}; + ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp); + + auto distortion = prop.GetDistCoeffs(); + std::vector distortionView{distortion.data(), + distortion.data() + distortion.size()}; + ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp); + + ts.heartbeatPublisher.Set(heartbeatCounter++, recieveTimestamp); + } + SimCameraProperties prop; + + private: + PhotonCamera* cam; + + NTTopicSet ts{}; + uint64_t heartbeatCounter{0}; + + uint64_t nextNTEntryTime{wpi::Now()}; + + units::meter_t maxSightRange{std::numeric_limits::max()}; + static constexpr double kDefaultMinAreaPx{100}; + double minTargetAreaPercent; + + frc::AprilTagFieldLayout tagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)}; + + cs::CvSource videoSimRaw; + cv::Mat videoSimFrameRaw{}; + bool videoSimRawEnabled{true}; + bool videoSimWireframeEnabled{false}; + double videoSimWireframeResolution{0.1}; + cs::CvSource videoSimProcessed; + cv::Mat videoSimFrameProcessed{}; + bool videoSimProcEnabled{true}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h new file mode 100644 index 0000000000..f3a5d73677 --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h @@ -0,0 +1,475 @@ +/* + * MIT License + * + * Copyright (c) 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 +#include +#include +#include +#include + +namespace photon { +class SimCameraProperties { + public: + SimCameraProperties() { SetCalibration(960, 720, frc::Rotation2d{90_deg}); } + SimCameraProperties(std::string path, int width, int height) {} + void SetCalibration(int width, int height, frc::Rotation2d fovDiag) { + if (fovDiag.Degrees() < 1_deg || fovDiag.Degrees() > 179_deg) { + fovDiag = frc::Rotation2d{ + std::clamp(fovDiag.Degrees(), 1_deg, 179_deg)}; + FRC_ReportError( + frc::err::Error, + "Requested invalid FOV! Clamping between (1, 179) degrees..."); + } + double resDiag = std::sqrt(width * width + height * height); + double diagRatio = units::math::tan(fovDiag.Radians() / 2.0); + frc::Rotation2d fovWidth{ + units::radian_t{std::atan(diagRatio * (width / resDiag)) * 2}}; + frc::Rotation2d fovHeight{ + units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}}; + + Eigen::Matrix newDistCoeffs; + newDistCoeffs << 0, 0, 0, 0, 0; + + double cx = width / 2.0 - 0.5; + double cy = height / 2.0 - 0.5; + + double fx = cx / units::math::tan(fovWidth.Radians() / 2.0); + double fy = cy / units::math::tan(fovHeight.Radians() / 2.0); + + Eigen::Matrix newCamIntrinsics; + newCamIntrinsics << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; + SetCalibration(width, height, newCamIntrinsics, newDistCoeffs); + } + + void SetCalibration(int width, int height, + const Eigen::Matrix& newCamIntrinsics, + const Eigen::Matrix& newDistCoeffs) { + resWidth = width; + resHeight = height; + camIntrinsics = newCamIntrinsics; + distCoeffs = newDistCoeffs; + + std::array p{ + frc::Translation3d{ + 1_m, + frc::Rotation3d{0_rad, 0_rad, + (GetPixelYaw(0) + frc::Rotation2d{units::radian_t{ + -std::numbers::pi / 2.0}}) + .Radians()}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, 0_rad, + (GetPixelYaw(width) + + frc::Rotation2d{ + units::radian_t{std::numbers::pi / 2.0}}) + .Radians()}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, + (GetPixelPitch(0) + + frc::Rotation2d{ + units::radian_t{std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, + (GetPixelPitch(height) + + frc::Rotation2d{ + units::radian_t{-std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, + }; + viewplanes.clear(); + for (size_t i = 0; i < p.size(); i++) { + viewplanes.emplace_back(Eigen::Matrix{ + p[i].X().to(), p[i].Y().to(), p[i].Z().to()}); + } + } + + void SetCalibError(double newAvgErrorPx, double newErrorStdDevPx) { + avgErrorPx = newAvgErrorPx; + errorStdDevPx = newErrorStdDevPx; + } + + void SetFPS(units::hertz_t fps) { + frameSpeed = units::math::max(1 / fps, exposureTime); + } + + void SetExposureTime(units::second_t newExposureTime) { + exposureTime = newExposureTime; + frameSpeed = units::math::max(frameSpeed, exposureTime); + } + + void SetAvgLatency(units::second_t newAvgLatency) { + avgLatency = newAvgLatency; + } + + void SetLatencyStdDev(units::second_t newLatencyStdDev) { + latencyStdDev = newLatencyStdDev; + } + + int GetResWidth() const { return resWidth; } + + int GetResHeight() const { return resHeight; } + + int GetResArea() const { return resWidth * resHeight; } + + double GetAspectRatio() const { + return static_cast(resWidth) / static_cast(resHeight); + } + + Eigen::Matrix GetIntrinsics() const { return camIntrinsics; } + + Eigen::Matrix GetDistCoeffs() const { return distCoeffs; } + + units::hertz_t GetFPS() const { return 1 / frameSpeed; } + + units::second_t GetFrameSpeed() const { return frameSpeed; } + + units::second_t GetExposureTime() const { return exposureTime; } + + units::second_t GetAverageLatency() const { return avgLatency; } + + units::second_t GetLatencyStdDev() const { return latencyStdDev; } + + double GetContourAreaPercent(const std::vector& points) { + return cv::contourArea(photon::OpenCVHelp::GetConvexHull(points)) / + GetResArea() * 100; + } + + frc::Rotation2d GetPixelYaw(double pixelX) const { + double fx = camIntrinsics(0, 0); + double cx = camIntrinsics(0, 2); + double xOffset = cx - pixelX; + return frc::Rotation2d{fx, xOffset}; + } + + frc::Rotation2d GetPixelPitch(double pixelY) const { + double fy = camIntrinsics(1, 1); + double cy = camIntrinsics(1, 2); + double yOffset = cy - pixelY; + return frc::Rotation2d{fy, -yOffset}; + } + + frc::Rotation3d GetPixelRot(const cv::Point2d& point) const { + return frc::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(), + GetPixelYaw(point.x).Radians()}; + } + + frc::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const { + double fx = camIntrinsics(0, 0); + double cx = camIntrinsics(0, 2); + double xOffset = cx - point.x; + + double fy = camIntrinsics(1, 1); + double cy = camIntrinsics(1, 2); + double yOffset = cy - point.y; + + frc::Rotation2d yaw{fx, xOffset}; + frc::Rotation2d pitch{fy / std::cos(std::atan(xOffset / fx)), -yOffset}; + return frc::Rotation3d{0_rad, pitch.Radians(), yaw.Radians()}; + } + + frc::Rotation2d GetHorizFOV() const { + frc::Rotation2d left = GetPixelYaw(0); + frc::Rotation2d right = GetPixelYaw(resWidth); + return left - right; + } + + frc::Rotation2d GetVertFOV() const { + frc::Rotation2d above = GetPixelPitch(0); + frc::Rotation2d below = GetPixelPitch(resHeight); + return below - above; + } + + frc::Rotation2d GetDiagFOV() const { + return frc::Rotation2d{ + units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())}; + } + + std::pair, std::optional> GetVisibleLine( + const RotTrlTransform3d& camRt, const frc::Translation3d& a, + const frc::Translation3d& b) const { + frc::Translation3d relA = camRt.Apply(a); + frc::Translation3d relB = camRt.Apply(b); + + if (relA.X() <= 0_m && relB.X() <= 0_m) { + return {std::nullopt, std::nullopt}; + } + + Eigen::Matrix av{relA.X().to(), relA.Y().to(), + relA.Z().to()}; + Eigen::Matrix bv{relB.X().to(), relB.Y().to(), + relB.Z().to()}; + Eigen::Matrix abv = bv - av; + + bool aVisible = true; + bool bVisible = true; + for (size_t i = 0; i < viewplanes.size(); i++) { + Eigen::Matrix normal = viewplanes[i]; + double aVisibility = av.dot(normal); + if (aVisibility < 0) { + aVisible = false; + } + double bVisibility = bv.dot(normal); + if (bVisibility < 0) { + bVisible = false; + } + if (aVisibility <= 0 && bVisibility <= 0) { + return {std::nullopt, std::nullopt}; + } + } + + if (aVisible && bVisible) { + return {0, 1}; + } + + std::array intersections{std::nan(""), std::nan(""), + std::nan(""), std::nan("")}; + std::vector>> ipts{ + {std::nullopt, std::nullopt, std::nullopt, std::nullopt}}; + + for (size_t i = 0; i < viewplanes.size(); i++) { + Eigen::Matrix normal = viewplanes[i]; + Eigen::Matrix a_projn{}; + a_projn = (av.dot(normal) / normal.dot(normal)) * normal; + + if (std::abs(abv.dot(normal)) < 1e-5) { + continue; + } + intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn)); + + Eigen::Matrix apv{}; + apv = intersections[i] * abv; + Eigen::Matrix intersectpt{}; + intersectpt = av + apv; + ipts[i] = intersectpt; + + for (size_t j = 1; j < viewplanes.size(); j++) { + int oi = (i + j) % viewplanes.size(); + Eigen::Matrix onormal = viewplanes[oi]; + if (intersectpt.dot(onormal) < 0) { + intersections[i] = std::nan(""); + ipts[i] = std::nullopt; + break; + } + } + + if (!ipts[i]) { + continue; + } + + for (int j = i - 1; j >= 0; j--) { + std::optional> oipt = ipts[j]; + if (!oipt) { + continue; + } + Eigen::Matrix diff{}; + diff = oipt.value() - intersectpt; + if (diff.cwiseAbs().maxCoeff() < 1e-4) { + intersections[i] = std::nan(""); + ipts[i] = std::nullopt; + break; + } + } + } + + double inter1 = std::nan(""); + double inter2 = std::nan(""); + for (double inter : intersections) { + if (!std::isnan(inter)) { + if (std::isnan(inter1)) { + inter1 = inter; + } else { + inter2 = inter; + } + } + } + + if (!std::isnan(inter2)) { + double max = std::max(inter1, inter2); + double min = std::min(inter1, inter2); + if (aVisible) { + min = 0; + } + if (bVisible) { + max = 1; + } + return {min, max}; + } else if (!std::isnan(inter1)) { + if (aVisible) { + return {0, inter1}; + } + if (bVisible) { + return {inter1, 1}; + } + return {inter1, std::nullopt}; + } else { + return {std::nullopt, std::nullopt}; + } + } + + std::vector EstPixelNoise( + const std::vector& points) { + if (avgErrorPx == 0 && errorStdDevPx == 0) { + return points; + } + + std::vector noisyPts; + noisyPts.reserve(points.size()); + for (size_t i = 0; i < points.size(); i++) { + cv::Point2f p = points[i]; + float error = avgErrorPx + gaussian(generator) * errorStdDevPx; + float errorAngle = + uniform(generator) * 2 * std::numbers::pi - std::numbers::pi; + noisyPts.emplace_back(cv::Point2f{p.x + error * std::cos(errorAngle), + p.y + error * std::sin(errorAngle)}); + } + return noisyPts; + } + + units::second_t EstLatency() { + return units::math::max(avgLatency + gaussian(generator) * latencyStdDev, + 0_s); + } + + units::second_t EstSecUntilNextFrame() { + return frameSpeed + units::math::max(0_s, EstLatency() - frameSpeed); + } + + static SimCameraProperties PERFECT_90DEG() { return SimCameraProperties{}; } + + static SimCameraProperties PI4_LIFECAM_320_240() { + SimCameraProperties prop{}; + prop.SetCalibration( + 320, 240, + (Eigen::MatrixXd(3, 3) << 328.2733242048587, 0.0, 164.8190261141906, + 0.0, 318.0609794305216, 123.8633838438093, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{ + 0.09957946553445934, -0.9166265114485799, 0.0019519890627236526, + -0.0036071725380870333, 1.5627234622420942}); + prop.SetCalibError(0.21, 0.0124); + prop.SetFPS(30_Hz); + prop.SetAvgLatency(30_ms); + prop.SetLatencyStdDev(10_ms); + return prop; + } + + static SimCameraProperties PI4_LIFECAM_640_480() { + SimCameraProperties prop{}; + prop.SetCalibration( + 640, 480, + (Eigen::MatrixXd(3, 3) << 669.1428078983059, 0.0, 322.53377249329213, + 0.0, 646.9843137061716, 241.26567383784163, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{ + 0.12788470750464645, -1.2350335805796528, 0.0024990767286192732, + -0.0026958287600230705, 2.2951386729115537}); + prop.SetCalibError(0.26, 0.046); + prop.SetFPS(15_Hz); + prop.SetAvgLatency(65_ms); + prop.SetLatencyStdDev(15_ms); + return prop; + } + + static SimCameraProperties LL2_640_480() { + SimCameraProperties prop{}; + prop.SetCalibration( + 640, 480, + (Eigen::MatrixXd(3, 3) << 511.22843367007755, 0.0, 323.62049380211096, + 0.0, 514.5452336723849, 261.8827920543568, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.1917469998873756, -0.5142936883324216, + 0.012461562046896614, 0.0014084973492408186, + 0.35160648971214437}); + prop.SetCalibError(0.25, 0.05); + prop.SetFPS(15_Hz); + prop.SetAvgLatency(35_ms); + prop.SetLatencyStdDev(8_ms); + return prop; + } + + static SimCameraProperties LL2_960_720() { + SimCameraProperties prop{}; + prop.SetCalibration( + 960, 720, + (Eigen::MatrixXd(3, 3) << 769.6873145148892, 0.0, 486.1096609458122, + 0.0, 773.8164483705323, 384.66071662358354, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.189462064814501, -0.49903003669627627, + 0.007468423590519429, 0.002496885298683693, + 0.3443122090208624}); + prop.SetCalibError(0.35, 0.10); + prop.SetFPS(10_Hz); + prop.SetAvgLatency(50_ms); + prop.SetLatencyStdDev(15_ms); + return prop; + } + + static SimCameraProperties LL2_1280_720() { + SimCameraProperties prop{}; + prop.SetCalibration( + 1280, 720, + (Eigen::MatrixXd(3, 3) << 1011.3749416937393, 0.0, 645.4955139388737, + 0.0, 1008.5391755084075, 508.32877656020196, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.13730101577061535, -0.2904345656989261, + 8.32475714507539E-4, -3.694397782014239E-4, + 0.09487962227027584}); + prop.SetCalibError(0.37, 0.06); + prop.SetFPS(7_Hz); + prop.SetAvgLatency(60_ms); + prop.SetLatencyStdDev(20_ms); + return prop; + } + + private: + std::mt19937 generator{std::random_device{}()}; + std::normal_distribution gaussian{0.0, 1.0}; + std::uniform_real_distribution uniform{0.0, 1.0}; + + int resWidth; + int resHeight; + Eigen::Matrix camIntrinsics; + Eigen::Matrix distCoeffs; + double avgErrorPx; + double errorStdDevPx; + units::second_t frameSpeed{0}; + units::second_t exposureTime{0}; + units::second_t avgLatency{0}; + units::second_t latencyStdDev{0}; + std::vector> viewplanes{}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h new file mode 100644 index 0000000000..bb2c453a88 --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h @@ -0,0 +1,432 @@ +/* + * MIT License + * + * Copyright (c) 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 +#include +#include +#include + +#include "SimCameraProperties.h" +#include "photon/estimation/RotTrlTransform3d.h" + +namespace mathutil { +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} +} // namespace mathutil + +namespace photon { +namespace VideoSimUtil { +static constexpr int kNumTags16h5 = 30; + +static constexpr units::meter_t fieldLength{16.54175_m}; +static constexpr units::meter_t fieldWidth{8.0137_m}; + +static cv::Mat Get16h5TagImage(int id) { + wpi::RawFrame frame = frc::AprilTag::Generate16h5AprilTagImage(id); + cv::Mat markerImage{frame.height, frame.width, CV_8UC1, frame.data}; + cv::Mat markerClone = markerImage.colRange(0, frame.dataLength).clone(); + return markerClone; +} + +static std::unordered_map LoadAprilTagImages() { + std::unordered_map retVal{}; + for (int i = 0; i < kNumTags16h5; i++) { + cv::Mat tagImage = Get16h5TagImage(i); + retVal[i] = tagImage; + } + return retVal; +} + +static std::vector GetImageCorners(const cv::Size& size) { + std::vector retVal{}; + retVal.emplace_back(cv::Point2f{-0.5f, -0.5f}); + retVal.emplace_back(cv::Point2f{size.width - 0.5f, -0.5f}); + retVal.emplace_back(cv::Point2f{size.width - 0.5f, size.height - 0.5f}); + retVal.emplace_back(cv::Point2f{-0.5f, size.height - 0.5f}); + return retVal; +} + +static std::vector Get16h5MarkerPts(int scale) { + cv::Rect2f roi16h5{cv::Point2f{1, 1}, cv::Point2f{6, 6}}; + roi16h5.x *= scale; + roi16h5.y *= scale; + roi16h5.width *= scale; + roi16h5.height *= scale; + std::vector pts = GetImageCorners(roi16h5.size()); + for (size_t i = 0; i < pts.size(); i++) { + cv::Point2f pt = pts[i]; + pts[i] = cv::Point2f{roi16h5.tl().x + pt.x, roi16h5.tl().y + pt.y}; + } + return pts; +} + +static std::vector Get16h5MarkerPts() { + return Get16h5MarkerPts(1); +} + +static const std::unordered_map kTag16h5Images = + LoadAprilTagImages(); +static const std::vector kTag16h5MarkPts = Get16h5MarkerPts(); + +[[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video, + const SimCameraProperties& prop) { + video.SetResolution(prop.GetResWidth(), prop.GetResHeight()); + video.SetFPS(prop.GetFPS().to()); +} + +[[maybe_unused]] static void Warp165h5TagImage( + int tagId, const std::vector& dstPoints, bool antialiasing, + cv::Mat& destination) { + if (!kTag16h5Images.contains(tagId)) { + return; + } + cv::Mat tagImage = kTag16h5Images.at(tagId); + std::vector tagPoints{kTag16h5MarkPts}; + std::vector tagImageCorners{GetImageCorners(tagImage.size())}; + std::vector dstPointMat = dstPoints; + cv::Rect boundingRect = cv::boundingRect(dstPointMat); + cv::Mat perspecTrf = cv::getPerspectiveTransform(tagPoints, dstPointMat); + std::vector extremeCorners{}; + cv::perspectiveTransform(tagImageCorners, extremeCorners, perspecTrf); + boundingRect = cv::boundingRect(extremeCorners); + + double warpedContourArea = cv::contourArea(extremeCorners); + double warpedTagUpscale = + std::sqrt(warpedContourArea) / std::sqrt(tagImage.size().area()); + int warpStrat = cv::INTER_NEAREST; + + int supersampling = 6; + supersampling = static_cast(std::ceil(supersampling / warpedTagUpscale)); + supersampling = std::max(std::min(supersampling, 8), 1); + + cv::Mat scaledTagImage{}; + if (warpedTagUpscale > 2.0) { + warpStrat = cv::INTER_LINEAR; + int scaleFactor = static_cast(warpedTagUpscale / 3.0) + 2; + scaleFactor = std::max(std::min(scaleFactor, 40), 1); + scaleFactor *= supersampling; + cv::resize(tagImage, scaledTagImage, cv::Size{}, scaleFactor, scaleFactor, + cv::INTER_NEAREST); + tagPoints = Get16h5MarkerPts(scaleFactor); + } else { + scaledTagImage = tagImage; + } + + boundingRect.x -= 1; + boundingRect.y -= 1; + boundingRect.width += 2; + boundingRect.height += 2; + if (boundingRect.x < 0) { + boundingRect.width += boundingRect.x; + boundingRect.x = 0; + } + if (boundingRect.y < 0) { + boundingRect.height += boundingRect.y; + boundingRect.y = 0; + } + boundingRect.width = + std::min(destination.size().width - boundingRect.x, boundingRect.width); + boundingRect.height = + std::min(destination.size().height - boundingRect.y, boundingRect.height); + if (boundingRect.width <= 0 || boundingRect.height <= 0) { + return; + } + + std::vector scaledDstPts{}; + if (supersampling > 1) { + cv::multiply(dstPointMat, + cv::Scalar{static_cast(supersampling), + static_cast(supersampling)}, + scaledDstPts); + boundingRect.x *= supersampling; + boundingRect.y *= supersampling; + boundingRect.width *= supersampling; + boundingRect.height *= supersampling; + } else { + scaledDstPts = dstPointMat; + } + + cv::subtract(scaledDstPts, + cv::Scalar{static_cast(boundingRect.tl().x), + static_cast(boundingRect.tl().y)}, + scaledDstPts); + perspecTrf = cv::getPerspectiveTransform(tagPoints, scaledDstPts); + + cv::Mat tempRoi{}; + cv::warpPerspective(scaledTagImage, tempRoi, perspecTrf, boundingRect.size(), + warpStrat); + + if (supersampling > 1) { + boundingRect.x /= supersampling; + boundingRect.y /= supersampling; + boundingRect.width /= supersampling; + boundingRect.height /= supersampling; + cv::resize(tempRoi, tempRoi, boundingRect.size(), 0, 0, cv::INTER_AREA); + } + + cv::Mat tempMask{cv::Mat::zeros(tempRoi.size(), CV_8UC1)}; + cv::subtract(extremeCorners, + cv::Scalar{static_cast(boundingRect.tl().x), + static_cast(boundingRect.tl().y)}, + extremeCorners); + cv::Point2f tempCenter{}; + tempCenter.x = + std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0, + [extremeCorners](float acc, const cv::Point2f& p2) { + return acc + p2.x / extremeCorners.size(); + }); + tempCenter.y = + std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0, + [extremeCorners](float acc, const cv::Point2f& p2) { + return acc + p2.y / extremeCorners.size(); + }); + + for (auto& corner : extremeCorners) { + float xDiff = corner.x - tempCenter.x; + float yDiff = corner.y - tempCenter.y; + xDiff += 1 * mathutil::sgn(xDiff); + yDiff += 1 * mathutil::sgn(yDiff); + corner = cv::Point2f{tempCenter.x + xDiff, tempCenter.y + yDiff}; + } + + std::vector extremeCornerInt{extremeCorners.begin(), + extremeCorners.end()}; + cv::fillConvexPoly(tempMask, extremeCornerInt, cv::Scalar{255}); + + cv::copyTo(tempRoi, destination(boundingRect), tempMask); +} + +static double GetScaledThickness(double thickness480p, + const cv::Mat& destinationMat) { + double scaleX = destinationMat.size().width / 640.0; + double scaleY = destinationMat.size().height / 480.0; + double minScale = std::min(scaleX, scaleY); + return std::max(thickness480p * minScale, 1.0); +} + +[[maybe_unused]] static void DrawInscribedEllipse( + const std::vector& dstPoints, const cv::Scalar& color, + cv::Mat& destination) { + cv::RotatedRect rect = OpenCVHelp::GetMinAreaRect(dstPoints); + cv::ellipse(destination, rect, color, -1, cv::LINE_AA); +} + +static void DrawPoly(const std::vector& dstPoints, int thickness, + const cv::Scalar& color, bool isClosed, + cv::Mat& destination) { + std::vector intDstPoints{dstPoints.begin(), dstPoints.end()}; + std::vector> listOfListOfPoints; + listOfListOfPoints.emplace_back(intDstPoints); + if (thickness > 0) { + cv::polylines(destination, listOfListOfPoints, isClosed, color, thickness, + cv::LINE_AA); + } else { + cv::fillPoly(destination, listOfListOfPoints, color, cv::LINE_AA); + } +} + +[[maybe_unused]] static void DrawTagDetection( + int id, const std::vector& dstPoints, cv::Mat& destination) { + double thickness = GetScaledThickness(1, destination); + DrawPoly(dstPoints, static_cast(thickness), cv::Scalar{0, 0, 255}, true, + destination); + cv::Rect2d rect{cv::boundingRect(dstPoints)}; + cv::Point2d textPt{rect.x + rect.width, rect.y}; + textPt.x += thickness; + textPt.y += thickness; + cv::putText(destination, std::to_string(id), textPt, cv::FONT_HERSHEY_PLAIN, + 1.5 * thickness, cv::Scalar{0, 200, 0}, + static_cast(thickness), cv::LINE_AA); +} + +static std::vector> GetFieldWallLines() { + std::vector> list; + + const units::meter_t sideHt = 19.5_in; + const units::meter_t driveHt = 35_in; + const units::meter_t topHt = 78_in; + + // field floor + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, 0_m}, + frc::Translation3d{fieldLength, 0_m, 0_m}, + frc::Translation3d{fieldLength, fieldWidth, 0_m}, + frc::Translation3d{0_m, fieldWidth, 0_m}, + frc::Translation3d{0_m, 0_m, 0_m}}); + + // right side wall + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Translation3d{0_m, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, 0_m}}); + + // red driverstation + list.emplace_back(std::vector{ + frc::Translation3d{fieldLength, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, topHt}, + frc::Translation3d{fieldLength, fieldWidth, topHt}, + frc::Translation3d{fieldLength, fieldWidth, sideHt}, + }); + list.emplace_back(std::vector{ + frc::Translation3d{fieldLength, 0_m, driveHt}, + frc::Translation3d{fieldLength, fieldWidth, driveHt}}); + + // left side wall + list.emplace_back(std::vector{ + frc::Translation3d{0_m, fieldWidth, 0_m}, + frc::Translation3d{0_m, fieldWidth, sideHt}, + frc::Translation3d{fieldLength, fieldWidth, sideHt}, + frc::Translation3d{fieldLength, fieldWidth, 0_m}}); + + // blue driverstation + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, sideHt}, + frc::Translation3d{0_m, 0_m, topHt}, + frc::Translation3d{0_m, fieldWidth, topHt}, + frc::Translation3d{0_m, fieldWidth, sideHt}, + }); + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, driveHt}, + frc::Translation3d{0_m, fieldWidth, driveHt}}); + + return list; +} + +static std::vector> GetFieldFloorLines( + int subdivisions) { + std::vector> list; + const units::meter_t subLength = fieldLength / subdivisions; + const units::meter_t subWidth = fieldWidth / subdivisions; + + for (int i = 0; i < subdivisions; i++) { + list.emplace_back(std::vector{ + frc::Translation3d{0_m, subWidth * (i + 1), 0_m}, + frc::Translation3d{fieldLength, subWidth * (i + 1), 0_m}}); + list.emplace_back(std::vector{ + frc::Translation3d{subLength * (i + 1), 0_m, 0_m}, + frc::Translation3d{subLength * (i + 1), fieldWidth, 0_m}}); + } + return list; +} + +static std::vector> PolyFrom3dLines( + const RotTrlTransform3d& camRt, const SimCameraProperties& prop, + const std::vector& trls, double resolution, + bool isClosed, cv::Mat& destination) { + resolution = std::hypot(destination.size().height, destination.size().width) * + resolution; + std::vector pts{trls}; + if (isClosed) { + pts.emplace_back(pts[0]); + } + std::vector> polyPointList{}; + + for (size_t i = 0; i < pts.size() - 1; i++) { + frc::Translation3d pta = pts[i]; + frc::Translation3d ptb = pts[i + 1]; + + std::pair, std::optional> inter = + prop.GetVisibleLine(camRt, pta, ptb); + if (!inter.second) { + continue; + } + + double inter1 = inter.first.value(); + double inter2 = inter.second.value(); + frc::Translation3d baseDelta = ptb - pta; + frc::Translation3d old_pta = pta; + if (inter1 > 0) { + pta = old_pta + baseDelta * inter1; + } + if (inter2 < 1) { + ptb = old_pta + baseDelta * inter2; + } + baseDelta = ptb - pta; + + std::vector poly = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, {pta, ptb}); + cv::Point2d pxa = poly[0]; + cv::Point2d pxb = poly[1]; + + double pxDist = std::hypot(pxb.x - pxa.x, pxb.y - pxa.y); + int subdivisions = static_cast(pxDist / resolution); + frc::Translation3d subDelta = baseDelta / (subdivisions + 1); + std::vector subPts{}; + for (int j = 0; j < subdivisions; j++) { + subPts.emplace_back(pta + (subDelta * (j + 1))); + } + if (subPts.size() > 0) { + std::vector toAdd = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, subPts); + poly.insert(poly.begin() + 1, toAdd.begin(), toAdd.end()); + } + + polyPointList.emplace_back(poly); + } + + return polyPointList; +} + +[[maybe_unused]] static void DrawFieldWireFrame( + const RotTrlTransform3d& camRt, const SimCameraProperties& prop, + double resolution, double wallThickness, const cv::Scalar& wallColor, + int floorSubdivisions, double floorThickness, const cv::Scalar& floorColor, + cv::Mat& destination) { + for (const auto& trls : GetFieldFloorLines(floorSubdivisions)) { + auto polys = + PolyFrom3dLines(camRt, prop, trls, resolution, false, destination); + for (const auto& poly : polys) { + DrawPoly(poly, + static_cast( + std::round(GetScaledThickness(floorThickness, destination))), + floorColor, false, destination); + } + } + for (const auto& trls : GetFieldWallLines()) { + auto polys = + PolyFrom3dLines(camRt, prop, trls, resolution, false, destination); + for (const auto& poly : polys) { + DrawPoly(poly, + static_cast( + std::round(GetScaledThickness(wallThickness, destination))), + wallColor, false, destination); + } + } +} +} // namespace VideoSimUtil +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h new file mode 100644 index 0000000000..55988bc32f --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h @@ -0,0 +1,282 @@ +/* + * MIT License + * + * Copyright (c) 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 + +#include "photon/simulation/PhotonCameraSim.h" + +namespace photon { +class VisionSystemSim { + public: + explicit VisionSystemSim(std::string visionSystemName) { + std::string tableName = "VisionSystemSim-" + visionSystemName; + frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField); + } + std::optional GetCameraSim(std::string name) { + auto it = camSimMap.find(name); + if (it != camSimMap.end()) { + return std::make_optional(it->second); + } else { + return std::nullopt; + } + } + std::vector GetCameraSims() { + std::vector retVal; + for (auto const& cam : camSimMap) { + retVal.emplace_back(cam.second); + } + return retVal; + } + void AddCamera(PhotonCameraSim* cameraSim, + const frc::Transform3d& robotToCamera) { + auto found = + camSimMap.find(std::string{cameraSim->GetCamera()->GetCameraName()}); + if (found == camSimMap.end()) { + camSimMap[std::string{cameraSim->GetCamera()->GetCameraName()}] = + cameraSim; + camTrfMap.insert(std::make_pair( + std::move(cameraSim), + frc::TimeInterpolatableBuffer{bufferLength})); + camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(), + frc::Pose3d{} + robotToCamera); + } + } + void ClearCameras() { + camSimMap.clear(); + camTrfMap.clear(); + } + bool RemoveCamera(PhotonCameraSim* cameraSim) { + int numOfElementsRemoved = + camSimMap.erase(std::string{cameraSim->GetCamera()->GetCameraName()}); + if (numOfElementsRemoved == 1) { + return true; + } else { + return false; + } + } + std::optional GetRobotToCamera(PhotonCameraSim* cameraSim) { + return GetRobotToCamera(cameraSim, frc::Timer::GetFPGATimestamp()); + } + std::optional GetRobotToCamera(PhotonCameraSim* cameraSim, + units::second_t time) { + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + frc::TimeInterpolatableBuffer trfBuffer = + camTrfMap.at(cameraSim); + std::optional sample = trfBuffer.Sample(time); + if (!sample) { + return std::nullopt; + } else { + return std::make_optional( + frc::Transform3d{frc::Pose3d{}, sample.value_or(frc::Pose3d{})}); + } + } else { + return std::nullopt; + } + } + std::optional GetCameraPose(PhotonCameraSim* cameraSim) { + return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp()); + } + std::optional GetCameraPose(PhotonCameraSim* cameraSim, + units::second_t time) { + auto robotToCamera = GetRobotToCamera(cameraSim, time); + if (!robotToCamera) { + return std::nullopt; + } else { + return std::make_optional(GetRobotPose(time) + robotToCamera.value()); + } + } + bool AdjustCamera(PhotonCameraSim* cameraSim, + const frc::Transform3d& robotToCamera) { + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(), + frc::Pose3d{} + robotToCamera); + return true; + } else { + return false; + } + } + void ResetCameraTransforms() { + for (const auto& pair : camTrfMap) { + ResetCameraTransforms(pair.first); + } + } + bool ResetCameraTransforms(PhotonCameraSim* cameraSim) { + units::second_t now = frc::Timer::GetFPGATimestamp(); + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + auto trfBuffer = camTrfMap.at(cameraSim); + frc::Transform3d lastTrf{frc::Pose3d{}, + trfBuffer.Sample(now).value_or(frc::Pose3d{})}; + trfBuffer.Clear(); + AdjustCamera(cameraSim, lastTrf); + return true; + } else { + return false; + } + } + std::vector GetVisionTargets() { + std::vector all{}; + for (const auto& entry : targetSets) { + for (const auto& target : entry.second) { + all.emplace_back(target); + } + } + return all; + } + std::vector GetVisionTargets(std::string type) { + return targetSets[type]; + } + void AddVisionTargets(const std::vector& targets) { + AddVisionTargets("targets", targets); + } + void AddVisionTargets(std::string type, + const std::vector& targets) { + if (!targetSets.contains(type)) { + targetSets[type] = std::vector{}; + } + for (const auto& tgt : targets) { + targetSets[type].emplace_back(tgt); + } + } + void AddAprilTags(const frc::AprilTagFieldLayout& layout) { + std::vector targets; + for (const frc::AprilTag& tag : layout.GetTags()) { + targets.emplace_back(VisionTargetSim{layout.GetTagPose(tag.ID).value(), + photon::kAprilTag16h5, tag.ID}); + } + AddVisionTargets("apriltag", targets); + } + void ClearVisionTargets() { targetSets.clear(); } + void ClearAprilTags() { RemoveVisionTargets("apriltag"); } + void RemoveVisionTargets(std::string type) { targetSets.erase(type); } + std::vector RemoveVisionTargets( + const std::vector& targets) { + std::vector removedList; + for (auto& entry : targetSets) { + for (auto target : entry.second) { + auto it = std::find(targets.begin(), targets.end(), target); + if (it != targets.end()) { + removedList.emplace_back(target); + entry.second.erase(it); + } + } + } + return removedList; + } + frc::Pose3d GetRobotPose() { + return GetRobotPose(frc::Timer::GetFPGATimestamp()); + } + frc::Pose3d GetRobotPose(units::second_t timestamp) { + return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{}); + } + void ResetRobotPose(const frc::Pose2d& robotPose) { + ResetRobotPose(frc::Pose3d{robotPose}); + } + void ResetRobotPose(const frc::Pose3d& robotPose) { + robotPoseBuffer.Clear(); + robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose); + } + frc::Field2d& GetDebugField() { return dbgField; } + void Update(const frc::Pose2d& robotPose) { Update(frc::Pose3d{robotPose}); } + void Update(const frc::Pose3d& robotPose) { + for (auto& set : targetSets) { + std::vector posesToAdd{}; + for (auto& target : set.second) { + posesToAdd.emplace_back(target.GetPose().ToPose2d()); + } + dbgField.GetObject(set.first)->SetPoses(posesToAdd); + } + + units::second_t now = frc::Timer::GetFPGATimestamp(); + robotPoseBuffer.AddSample(now, robotPose); + dbgField.SetRobotPose(robotPose.ToPose2d()); + + std::vector allTargets{}; + for (const auto& set : targetSets) { + for (const auto& target : set.second) { + allTargets.emplace_back(target); + } + } + + std::vector visTgtPoses2d{}; + std::vector cameraPoses2d{}; + bool processed{false}; + for (const auto& entry : camSimMap) { + auto camSim = entry.second; + auto optTimestamp = camSim->ConsumeNextEntryTime(); + if (!optTimestamp) { + continue; + } else { + processed = true; + } + uint64_t timestampNt = optTimestamp.value(); + units::second_t latency = camSim->prop.EstLatency(); + units::second_t timestampCapture = + units::microsecond_t{static_cast(timestampNt)} - latency; + + frc::Pose3d lateRobotPose = GetRobotPose(timestampCapture); + frc::Pose3d lateCameraPose = + lateRobotPose + GetRobotToCamera(camSim, timestampCapture).value(); + cameraPoses2d.push_back(lateCameraPose.ToPose2d()); + + auto camResult = camSim->Process(latency, lateCameraPose, allTargets); + camSim->SubmitProcessedFrame(camResult, timestampNt); + for (const auto& target : camResult.GetTargets()) { + auto trf = target.GetBestCameraToTarget(); + if (trf == kEmptyTrf) { + continue; + } + visTgtPoses2d.push_back(lateCameraPose.TransformBy(trf).ToPose2d()); + } + } + if (processed) { + dbgField.GetObject("visibleTargetPoses")->SetPoses(visTgtPoses2d); + } + if (cameraPoses2d.size() != 0) { + dbgField.GetObject("cameras")->SetPoses(cameraPoses2d); + } + } + + private: + std::unordered_map camSimMap{}; + static constexpr units::second_t bufferLength{1.5_s}; + std::unordered_map> + camTrfMap; + frc::TimeInterpolatableBuffer robotPoseBuffer{bufferLength}; + std::unordered_map> targetSets{}; + frc::Field2d dbgField{}; + const frc::Transform3d kEmptyTrf{}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h new file mode 100644 index 0000000000..f8d4d6676b --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h @@ -0,0 +1,73 @@ +/* + * MIT License + * + * Copyright (c) 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 "photon/estimation/TargetModel.h" + +namespace photon { +class VisionTargetSim { + public: + VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model) + : fiducialId(-1), pose(pose), model(model) {} + VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model, int id) + : fiducialId(id), pose(pose), model(model) {} + void SetPose(const frc::Pose3d& newPose) { pose = newPose; } + void SetModel(const TargetModel& newModel) { model = newModel; } + frc::Pose3d GetPose() const { return pose; } + TargetModel GetModel() const { return model; } + std::vector GetFieldVertices() const { + return model.GetFieldVertices(pose); + } + int fiducialId; + + bool operator<(const VisionTargetSim& right) const { + return pose.Translation().Norm() < right.pose.Translation().Norm(); + } + + bool operator==(const VisionTargetSim& other) const { + return units::math::abs(pose.Translation().X() - + other.GetPose().Translation().X()) < 1_in && + units::math::abs(pose.Translation().Y() - + other.GetPose().Translation().Y()) < 1_in && + units::math::abs(pose.Translation().Z() - + other.GetPose().Translation().Z()) < 1_in && + units::math::abs(pose.Rotation().X() - + other.GetPose().Rotation().X()) < 1_deg && + units::math::abs(pose.Rotation().Y() - + other.GetPose().Rotation().Y()) < 1_deg && + units::math::abs(pose.Rotation().Z() - + other.GetPose().Rotation().Z()) < 1_deg && + model.GetIsPlanar() == other.GetModel().GetIsPlanar(); + } + + private: + frc::Pose3d pose; + TargetModel model; +}; +} // namespace photon diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp new file mode 100644 index 0000000000..1c3f9eac58 --- /dev/null +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -0,0 +1,461 @@ +/* + * MIT License + * + * Copyright (c) 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 "photon/PhotonUtils.h" +#include "photon/simulation/VisionSystemSim.h" + +class VisionSystemSimTest : public ::testing::Test { + void SetUp() override { + nt::NetworkTableInstance::GetDefault().StartServer(); + photon::PhotonCamera::SetVersionCheckEnabled(false); + } + + void TearDown() override {} +}; + +class VisionSystemSimTestWithParamsTest + : public VisionSystemSimTest, + public testing::WithParamInterface {}; +class VisionSystemSimTestDistanceParamsTest + : public VisionSystemSimTest, + public testing::WithParamInterface< + std::tuple> {}; + +TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{1.0_m, 1.0_m}, 3}}); + + // To the right, to the right + frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-70_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the right, to the right + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-95_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{90_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{65_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // Now kick, now kick + robotPose = frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + // Now kick, now kick + robotPose = + frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + // Now walk it by yourself + robotPose = + frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-179_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // Now walk it by yourself + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleVert1) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{3.0_m, 3.0_m}, 3}}); + + frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 5000_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleVert2) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + frc::Transform3d robotToCamera{ + frc::Translation3d{0_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, units::radian_t{-std::numbers::pi / 4}, 0_rad}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, robotToCamera); + cameraSim.prop.SetCalibration(1234, 1234, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 1736}}); + + frc::Pose2d robotPose{frc::Translation2d{13.98_m, 0_m}, + frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.1_m, 0.1_m}, 24}}); + + frc::Pose2d robotPose{frc::Translation2d{12_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(1.0); + cameraSim.SetMaxSightRange(10_m); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, photon::TargetModel{1_m, 1_m}, 25}}); + + frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + + robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m}, + frc::Rotation2d{-1 * GetParam()}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + ASSERT_NEAR(GetParam().to(), + camera.GetLatestResult().GetBestTarget().GetYaw(), 0.25); +} + +TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{120_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + + robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m}, + frc::Rotation2d{-1 * GetParam()}}; + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{}, + frc::Rotation3d{0_rad, units::degree_t{GetParam()}, 0_rad}}); + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + ASSERT_NEAR(GetParam().to(), + camera.GetLatestResult().GetBestTarget().GetPitch(), 0.25); +} + +INSTANTIATE_TEST_SUITE_P(AnglesTests, VisionSystemSimTestWithParamsTest, + testing::Values(-10_deg, -5_deg, -0_deg, -1_deg, + -2_deg, 5_deg, 7_deg, 10.23_deg)); + +TEST_P(VisionSystemSimTestDistanceParamsTest, 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::radian_t{std::numbers::pi * 0.98}}}; + 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}}; + photon::VisionSystemSim visionSysSim{ + "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho" + "wsyourdaygoingihopegoodhaveagreatrestofyourlife"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{160_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AdjustCamera(&cameraSim, robotToCamera); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 0}}); + visionSysSim.Update(robotPose); + + photon::PhotonPipelineResult res = camera.GetLatestResult(); + ASSERT_TRUE(res.HasTargets()); + photon::PhotonTrackedTarget target = res.GetBestTarget(); + + ASSERT_NEAR(0.0, target.GetYaw(), 0.5); + + units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget( + robotToCamera.Z(), targetPose.Z(), -pitchParam, + units::degree_t{target.GetPitch()}); + ASSERT_NEAR(dist.to(), + distParam.convert().to(), 0.25); +} + +INSTANTIATE_TEST_SUITE_P( + DistanceParamsTests, VisionSystemSimTestDistanceParamsTest, + 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(VisionSystemSimTest, TestMultipleTargets) { + frc::Pose3d targetPoseL{ + frc::Translation3d{15.98_m, 2_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + frc::Pose3d targetPoseC{ + frc::Translation3d{15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + frc::Pose3d targetPoseR{ + frc::Translation3d{15.98_m, -2_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 1}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 2}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 3}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 4}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 5}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 6}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 7}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 8}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 9}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 10}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 11}}); + + frc::Pose2d robotPose{frc::Translation2d{6_m, 0_m}, frc::Rotation2d{.25_deg}}; + visionSysSim.Update(robotPose); + photon::PhotonPipelineResult res = camera.GetLatestResult(); + ASSERT_TRUE(res.HasTargets()); + std::span tgtList = res.GetTargets(); + ASSERT_EQ(static_cast(11), tgtList.size()); +} + +TEST_F(VisionSystemSimTest, TestPoseEstimation) { + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + std::vector tagList; + tagList.emplace_back(frc::AprilTag{ + 0, frc::Pose3d{12_m, 3_m, 1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 1, frc::Pose3d{12_m, 1_m, -1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 2, frc::Pose3d{11_m, 0_m, 2_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + units::meter_t fieldLength{54}; + units::meter_t fieldWidth{27}; + frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; + frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{5_deg}}; + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag16h5, 0}}); + visionSysSim.Update(robotPose); + + Eigen::Matrix camEigen; + cv::cv2eigen(camera.GetCameraMatrix().value(), camEigen); + + Eigen::Matrix distEigen; + cv::cv2eigen(camera.GetDistCoeffs().value(), distEigen); + + auto camResults = camera.GetLatestResult(); + auto targetSpan = camResults.GetTargets(); + std::vector targets; + for (photon::PhotonTrackedTarget tar : targetSpan) { + targets.push_back(tar); + } + photon::PNPResult results = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets, layout, photon::kAprilTag16h5); + frc::Pose3d pose = frc::Pose3d{} + results.best; + ASSERT_NEAR(5, pose.X().to(), 0.01); + ASSERT_NEAR(1, pose.Y().to(), 0.01); + ASSERT_NEAR(0, pose.Z().to(), 0.01); + ASSERT_NEAR(units::degree_t{5}.convert().to(), + pose.Rotation().Z().to(), 0.01); + + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag16h5, 1}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[2].pose, photon::kAprilTag16h5, 2}}); + visionSysSim.Update(robotPose); + + auto camResults2 = camera.GetLatestResult(); + auto targetSpan2 = camResults2.GetTargets(); + std::vector targets2; + for (photon::PhotonTrackedTarget tar : targetSpan2) { + targets2.push_back(tar); + } + photon::PNPResult results2 = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets2, layout, photon::kAprilTag16h5); + frc::Pose3d pose2 = frc::Pose3d{} + results2.best; + ASSERT_NEAR(5, pose2.X().to(), 0.01); + ASSERT_NEAR(1, pose2.Y().to(), 0.01); + ASSERT_NEAR(0, pose2.Z().to(), 0.01); + ASSERT_NEAR(units::degree_t{5}.convert().to(), + pose2.Rotation().Z().to(), 0.01); +} diff --git a/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h new file mode 100644 index 0000000000..a0a0fb0e61 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h @@ -0,0 +1,61 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +namespace photon { +class CameraTargetRelation { + public: + const frc::Pose3d camPose; + const frc::Transform3d camToTarg; + const units::meter_t camToTargDist; + const units::meter_t camToTargDistXY; + const frc::Rotation2d camToTargYaw; + const frc::Rotation2d camToTargPitch; + + const frc::Rotation2d camToTargAngle; + + const frc::Transform3d targToCam; + const frc::Rotation2d targToCamYaw; + const frc::Rotation2d targToCamPitch; + + const frc::Rotation2d targToCamAngle; + + CameraTargetRelation(const frc::Pose3d& cameraPose, + const frc::Pose3d& targetPose) + : camPose(cameraPose), + camToTarg(frc::Transform3d{cameraPose, targetPose}), + camToTargDist(camToTarg.Translation().Norm()), + camToTargDistXY(units::math::hypot(camToTarg.Translation().X(), + camToTarg.Translation().Y())), + camToTargYaw(frc::Rotation2d{camToTarg.X().to(), + camToTarg.Y().to()}), + camToTargPitch(frc::Rotation2d{camToTargDistXY.to(), + -camToTarg.Z().to()}), + camToTargAngle(frc::Rotation2d{units::math::hypot( + camToTargYaw.Radians(), camToTargPitch.Radians())}), + targToCam(frc::Transform3d{targetPose, cameraPose}), + targToCamYaw(frc::Rotation2d{targToCam.X().to(), + targToCam.Y().to()}), + targToCamPitch(frc::Rotation2d{camToTargDistXY.to(), + -targToCam.Z().to()}), + targToCamAngle(frc::Rotation2d{units::math::hypot( + targToCamYaw.Radians(), targToCamPitch.Radians())}) {} +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h new file mode 100644 index 0000000000..2d79e661e0 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h @@ -0,0 +1,291 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "RotTrlTransform3d.h" + +#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT +#include +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/MultiTargetPNPResult.h" + +namespace photon { +namespace OpenCVHelp { + +static frc::Rotation3d NWU_TO_EDN{ + (Eigen::Matrix3d() << 0, -1, 0, 0, 0, -1, 1, 0, 0).finished()}; +static frc::Rotation3d EDN_TO_NWU{ + (Eigen::Matrix3d() << 0, 0, 1, -1, 0, 0, 0, -1, 0).finished()}; + +static std::vector GetConvexHull( + const std::vector& points) { + std::vector outputHull{}; + cv::convexHull(points, outputHull); + std::vector convexPoints; + for (size_t i = 0; i < outputHull.size(); i++) { + convexPoints.push_back(points[outputHull[i]]); + } + return convexPoints; +} + +static cv::RotatedRect GetMinAreaRect(const std::vector& points) { + return cv::minAreaRect(points); +} + +static frc::Translation3d TranslationNWUtoEDN(const frc::Translation3d& trl) { + return trl.RotateBy(NWU_TO_EDN); +} + +static frc::Rotation3d RotationNWUtoEDN(const frc::Rotation3d& rot) { + return -NWU_TO_EDN + (rot + NWU_TO_EDN); +} + +static std::vector TranslationToTVec( + const std::vector& translations) { + std::vector retVal; + retVal.reserve(translations.size()); + for (size_t i = 0; i < translations.size(); i++) { + frc::Translation3d trl = TranslationNWUtoEDN(translations[i]); + retVal.emplace_back(cv::Point3f{trl.X().to(), trl.Y().to(), + trl.Z().to()}); + } + return retVal; +} + +static std::vector RotationToRVec( + const frc::Rotation3d& rotation) { + std::vector retVal{}; + frc::Rotation3d rot = RotationNWUtoEDN(rotation); + retVal.emplace_back(cv::Point3d{ + rot.GetQuaternion().ToRotationVector()(0), + rot.GetQuaternion().ToRotationVector()(1), + rot.GetQuaternion().ToRotationVector()(2), + }); + return retVal; +} + +[[maybe_unused]] static cv::Point2f AvgPoint(std::vector points) { + if (points.size() == 0) { + return cv::Point2f{}; + } + cv::reduce(points, points, 0, cv::REDUCE_AVG); + return points[0]; +} + +[[maybe_unused]] static std::vector> PointsToCorners( + const std::vector& points) { + std::vector> retVal; + retVal.reserve(points.size()); + for (size_t i = 0; i < points.size(); i++) { + retVal.emplace_back(std::make_pair(points[i].x, points[i].y)); + } + return retVal; +} + +[[maybe_unused]] static std::vector CornersToPoints( + const std::vector>& corners) { + std::vector retVal; + retVal.reserve(corners.size()); + for (size_t i = 0; i < corners.size(); i++) { + retVal.emplace_back(cv::Point2f{corners[i].first, corners[i].second}); + } + return retVal; +} + +[[maybe_unused]] static cv::Rect GetBoundingRect( + const std::vector& points) { + return cv::boundingRect(points); +} + +static std::vector ProjectPoints( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + const RotTrlTransform3d& camRt, + const std::vector& objectTranslations) { + std::vector objectPoints = TranslationToTVec(objectTranslations); + std::vector rvec = RotationToRVec(camRt.GetRotation()); + std::vector tvec = TranslationToTVec({camRt.GetTranslation()}); + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + std::vector imagePoints{}; + cv::projectPoints(objectPoints, rvec, tvec, cameraMat, distCoeffsMat, + imagePoints); + return imagePoints; +} + +template +static std::vector ReorderCircular(const std::vector elements, + bool backwards, int shiftStart) { + size_t size = elements.size(); + int dir = backwards ? -1 : 1; + std::vector reordered{elements}; + for (size_t i = 0; i < size; i++) { + int index = (i * dir + shiftStart * dir) % size; + if (index < 0) { + index = size + index; + } + reordered[i] = elements[index]; + } + return reordered; +} + +static frc::Translation3d TranslationEDNToNWU(const frc::Translation3d& trl) { + return trl.RotateBy(EDN_TO_NWU); +} + +static frc::Rotation3d RotationEDNToNWU(const frc::Rotation3d& rot) { + return -EDN_TO_NWU + (rot + EDN_TO_NWU); +} + +static frc::Translation3d TVecToTranslation(const cv::Mat& tvecInput) { + cv::Vec3f data{}; + cv::Mat wrapped{tvecInput.rows, tvecInput.cols, CV_32F}; + tvecInput.convertTo(wrapped, CV_32F); + data = wrapped.at(cv::Point{0, 0}); + return TranslationEDNToNWU(frc::Translation3d{units::meter_t{data[0]}, + units::meter_t{data[1]}, + units::meter_t{data[2]}}); +} + +static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { + cv::Vec3f data{}; + cv::Mat wrapped{rvecInput.rows, rvecInput.cols, CV_32F}; + rvecInput.convertTo(wrapped, CV_32F); + data = wrapped.at(cv::Point{0, 0}); + return RotationEDNToNWU(frc::Rotation3d{units::radian_t{data[0]}, + units::radian_t{data[1]}, + units::radian_t{data[2]}}); +} + +[[maybe_unused]] static photon::PNPResult SolvePNP_Square( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + std::vector modelTrls, + std::vector imagePoints) { + modelTrls = ReorderCircular(modelTrls, true, -1); + imagePoints = ReorderCircular(imagePoints, true, -1); + std::vector objectMat = TranslationToTVec(modelTrls); + std::vector rvecs; + std::vector tvecs; + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F); + + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_32F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_32F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + + cv::Vec2d errors{}; + frc::Transform3d best{}; + std::optional alt{std::nullopt}; + + for (int tries = 0; tries < 2; tries++) { + cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs, + tvecs, false, cv::SOLVEPNP_IPPE_SQUARE, rvec, tvec, + reprojectionError); + + errors = reprojectionError.at(cv::Point{0, 0}); + best = frc::Transform3d{TVecToTranslation(tvecs.at(0)), + RVecToRotation(rvecs[0])}; + + if (tvecs.size() > 1) { + alt = frc::Transform3d{TVecToTranslation(tvecs.at(1)), + RVecToRotation(rvecs[1])}; + } + + if (!std::isnan(errors[0])) { + break; + } else { + cv::Point2f pt = imagePoints[0]; + pt.x -= 0.001f; + pt.y -= 0.001f; + imagePoints[0] = pt; + } + } + + if (std::isnan(errors[0])) { + fmt::print("SolvePNP_Square failed!\n"); + } + if (alt) { + photon::PNPResult result; + result.best = best; + result.alt = alt.value(); + result.ambiguity = errors[0] / errors[1]; + result.bestReprojErr = errors[0]; + result.altReprojErr = errors[1]; + result.isPresent = true; + return result; + } else { + photon::PNPResult result; + result.best = best; + result.bestReprojErr = errors[0]; + result.isPresent = true; + return result; + } +} + +[[maybe_unused]] static photon::PNPResult SolvePNP_SQPNP( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + std::vector modelTrls, + std::vector imagePoints) { + std::vector objectMat = TranslationToTVec(modelTrls); + std::vector rvecs{}; + std::vector tvecs{}; + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F); + + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + + float error = 0; + frc::Transform3d best{}; + + cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs, + tvecs, false, cv::SOLVEPNP_SQPNP, rvec, tvec, + reprojectionError); + + error = reprojectionError.at(cv::Point{0, 0}); + best = frc::Transform3d{TVecToTranslation(tvecs.at(0)), + RVecToRotation(rvecs[0])}; + + if (std::isnan(error)) { + fmt::print("SolvePNP_Square failed!\n"); + } + photon::PNPResult result; + result.best = best; + result.bestReprojErr = error; + result.isPresent = true; + return result; +} +} // namespace OpenCVHelp +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h new file mode 100644 index 0000000000..99e19bf2c3 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h @@ -0,0 +1,102 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +#include +#include +#include + +namespace photon { +class RotTrlTransform3d { + public: + RotTrlTransform3d(const frc::Rotation3d& rot, const frc::Translation3d& trl) + : trl(trl), rot(rot) {} + RotTrlTransform3d(const frc::Pose3d& initial, const frc::Pose3d& last) + : trl(last.Translation() - initial.Translation().RotateBy(rot)), + rot(last.Rotation() - initial.Rotation()) {} + explicit RotTrlTransform3d(const frc::Transform3d& trf) + : RotTrlTransform3d(trf.Rotation(), trf.Translation()) {} + RotTrlTransform3d() + : RotTrlTransform3d(frc::Rotation3d{}, frc::Translation3d{}) {} + + static RotTrlTransform3d MakeRelativeTo(const frc::Pose3d& pose) { + return RotTrlTransform3d{pose.Rotation(), pose.Translation()}.Inverse(); + } + + RotTrlTransform3d Inverse() const { + frc::Rotation3d invRot = -rot; + frc::Translation3d invTrl = -(trl.RotateBy(invRot)); + return RotTrlTransform3d{invRot, invTrl}; + } + + frc::Transform3d GetTransform() const { return frc::Transform3d{trl, rot}; } + + frc::Translation3d GetTranslation() const { return trl; } + + frc::Rotation3d GetRotation() const { return rot; } + + frc::Translation3d Apply(const frc::Translation3d& trlToApply) const { + return trlToApply.RotateBy(rot) + trl; + } + + std::vector ApplyTrls( + const std::vector& trls) const { + std::vector retVal; + retVal.reserve(trls.size()); + for (const auto& currentTrl : trls) { + retVal.emplace_back(Apply(currentTrl)); + } + return retVal; + } + + frc::Rotation3d Apply(const frc::Rotation3d& rotToApply) const { + return rotToApply + rot; + } + + std::vector ApplyTrls( + const std::vector& rots) const { + std::vector retVal; + retVal.reserve(rots.size()); + for (const auto& currentRot : rots) { + retVal.emplace_back(Apply(currentRot)); + } + return retVal; + } + + frc::Pose3d Apply(const frc::Pose3d& poseToApply) const { + return frc::Pose3d{Apply(poseToApply.Translation()), + Apply(poseToApply.Rotation())}; + } + + std::vector ApplyPoses( + const std::vector& poses) const { + std::vector retVal; + retVal.reserve(poses.size()); + for (const auto& currentPose : poses) { + retVal.emplace_back(Apply(currentPose)); + } + return retVal; + } + + private: + const frc::Translation3d trl; + const frc::Rotation3d rot; +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h new file mode 100644 index 0000000000..64392fdb11 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h @@ -0,0 +1,115 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +#include +#include + +#include "RotTrlTransform3d.h" + +namespace photon { +class TargetModel { + public: + TargetModel(units::meter_t width, units::meter_t height) + : vertices({frc::Translation3d{0_m, -width / 2.0, -height / 2.0}, + frc::Translation3d{0_m, width / 2.0, -height / 2.0}, + frc::Translation3d{0_m, width / 2.0, height / 2.0}, + frc::Translation3d{0_m, -width / 2.0, height / 2.0}}), + isPlanar(true), + isSpherical(false) {} + + TargetModel(units::meter_t length, units::meter_t width, + units::meter_t height) + : TargetModel({ + frc::Translation3d{length / 2.0, -width / 2.0, -height / 2.0}, + frc::Translation3d{length / 2.0, width / 2.0, -height / 2.0}, + frc::Translation3d{length / 2.0, width / 2.0, height / 2.0}, + frc::Translation3d{length / 2.0, -width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, -width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, width / 2.0, -height / 2.0}, + frc::Translation3d{-length / 2.0, -width / 2.0, -height / 2.0}, + }) {} + + explicit TargetModel(units::meter_t diameter) + : vertices({ + frc::Translation3d{0_m, -diameter / 2.0, 0_m}, + frc::Translation3d{0_m, 0_m, -diameter / 2.0}, + frc::Translation3d{0_m, diameter / 2.0, 0_m}, + frc::Translation3d{0_m, 0_m, diameter / 2.0}, + }), + isPlanar(false), + isSpherical(true) {} + + explicit TargetModel(const std::vector& verts) + : isSpherical(false) { + if (verts.size() <= 2) { + vertices = std::vector(); + isPlanar = false; + } else { + bool cornersPlanar = true; + for (const auto& corner : verts) { + if (corner.X() != 0_m) { + cornersPlanar = false; + } + } + isPlanar = cornersPlanar; + } + vertices = verts; + } + + std::vector GetFieldVertices( + const frc::Pose3d& targetPose) const { + RotTrlTransform3d basisChange{targetPose.Rotation(), + targetPose.Translation()}; + std::vector retVal; + retVal.reserve(vertices.size()); + for (const auto& vert : vertices) { + retVal.emplace_back(basisChange.Apply(vert)); + } + return retVal; + } + + static frc::Pose3d GetOrientedPose(const frc::Translation3d& tgtTrl, + const frc::Translation3d& cameraTrl) { + frc::Translation3d relCam = cameraTrl - tgtTrl; + frc::Rotation3d orientToCam = frc::Rotation3d{ + 0_rad, + frc::Rotation2d{units::math::hypot(relCam.X(), relCam.Y()).to(), + -relCam.Z().to()} + .Radians(), + frc::Rotation2d{relCam.X().to(), relCam.Y().to()} + .Radians()}; + return frc::Pose3d{tgtTrl, orientToCam}; + } + + std::vector GetVertices() const { return vertices; } + bool GetIsPlanar() const { return isPlanar; } + bool GetIsSpherical() const { return isSpherical; } + + private: + std::vector vertices; + bool isPlanar; + bool isSpherical; +}; + +static const TargetModel kAprilTag16h5{6_in, 6_in}; +static const TargetModel kAprilTag36h11{6.5_in, 6.5_in}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h new file mode 100644 index 0000000000..cb3ae3bdc5 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h @@ -0,0 +1,121 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include +#include + +#include +#include +#include + +#include "OpenCVHelp.h" +#include "TargetModel.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" + +namespace photon { +namespace VisionEstimation { + +static std::vector GetVisibleLayoutTags( + const std::vector& visTags, + const frc::AprilTagFieldLayout& layout) { + std::vector retVal{}; + for (const auto& tag : visTags) { + int id = tag.GetFiducialId(); + auto maybePose = layout.GetTagPose(id); + if (maybePose) { + retVal.emplace_back(frc::AprilTag{id, maybePose.value()}); + } + } + return retVal; +} + +static PNPResult EstimateCamPosePNP( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + const std::vector& visTags, + const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) { + if (visTags.size() == 0) { + return PNPResult(); + } + + std::vector> corners{}; + std::vector knownTags{}; + + for (const auto& tgt : visTags) { + int id = tgt.GetFiducialId(); + auto maybePose = layout.GetTagPose(id); + if (maybePose) { + knownTags.emplace_back(frc::AprilTag{id, maybePose.value()}); + auto currentCorners = tgt.GetDetectedCorners(); + corners.insert(corners.end(), currentCorners.begin(), + currentCorners.end()); + } + } + if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { + return PNPResult{}; + } + + std::vector points = OpenCVHelp::CornersToPoints(corners); + + if (knownTags.size() == 1) { + PNPResult camToTag = OpenCVHelp::SolvePNP_Square( + cameraMatrix, distCoeffs, tagModel.GetVertices(), points); + if (!camToTag.isPresent) { + return PNPResult{}; + } + frc::Pose3d bestPose = + knownTags[0].pose.TransformBy(camToTag.best.Inverse()); + frc::Pose3d altPose{}; + if (camToTag.ambiguity != 0) { + altPose = knownTags[0].pose.TransformBy(camToTag.alt.Inverse()); + } + frc::Pose3d o{}; + PNPResult result{}; + result.best = frc::Transform3d{o, bestPose}; + result.alt = frc::Transform3d{o, altPose}; + result.ambiguity = camToTag.ambiguity; + result.bestReprojErr = camToTag.bestReprojErr; + result.altReprojErr = camToTag.altReprojErr; + return result; + } else { + std::vector objectTrls{}; + for (const auto& tag : knownTags) { + auto verts = tagModel.GetFieldVertices(tag.pose); + objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); + } + PNPResult camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, + objectTrls, points); + if (!camToOrigin.isPresent) { + return PNPResult{}; + } else { + PNPResult result{}; + result.best = camToOrigin.best.Inverse(), + result.alt = camToOrigin.alt.Inverse(), + result.ambiguity = camToOrigin.ambiguity; + result.bestReprojErr = camToOrigin.bestReprojErr; + result.altReprojErr = camToOrigin.altReprojErr; + result.isPresent = true; + return result; + } + } +} + +} // namespace VisionEstimation +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h new file mode 100644 index 0000000000..ff647cd7cf --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h @@ -0,0 +1,100 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace photon { +class NTTopicSet { + public: + std::shared_ptr subTable; + nt::RawPublisher rawBytesEntry; + + nt::IntegerPublisher pipelineIndexPublisher; + nt::IntegerSubscriber pipelineIndexRequestSub; + + nt::BooleanTopic driverModeEntry; + nt::BooleanPublisher driverModePublisher; + nt::BooleanSubscriber driverModeSubscriber; + + nt::DoublePublisher latencyMillisEntry; + nt::BooleanPublisher hasTargetEntry; + nt::DoublePublisher targetPitchEntry; + nt::DoublePublisher targetYawEntry; + nt::DoublePublisher targetAreaEntry; + nt::DoubleArrayPublisher targetPoseEntry; + nt::DoublePublisher targetSkewEntry; + + nt::DoublePublisher bestTargetPosX; + nt::DoublePublisher bestTargetPosY; + + nt::IntegerTopic heartbeatTopic; + nt::IntegerPublisher heartbeatPublisher; + + nt::DoubleArrayPublisher cameraIntrinsicsPublisher; + nt::DoubleArrayPublisher cameraDistortionPublisher; + + void UpdateEntries() { + nt::PubSubOptions options; + options.periodic = 0.01; + options.sendAll = true; + rawBytesEntry = + subTable->GetRawTopic("rawBytes").Publish("rawBytes", options); + + pipelineIndexPublisher = + subTable->GetIntegerTopic("pipelineIndexState").Publish(); + pipelineIndexRequestSub = + subTable->GetIntegerTopic("pipelineIndexRequest").Subscribe(0); + + driverModePublisher = subTable->GetBooleanTopic("driverMode").Publish(); + driverModeSubscriber = + subTable->GetBooleanTopic("driverModeRequest").Subscribe(0); + + driverModeSubscriber.GetTopic().Publish().SetDefault(false); + + latencyMillisEntry = subTable->GetDoubleTopic("latencyMillis").Publish(); + hasTargetEntry = subTable->GetBooleanTopic("hasTargets").Publish(); + + targetPitchEntry = subTable->GetDoubleTopic("targetPitch").Publish(); + targetAreaEntry = subTable->GetDoubleTopic("targetArea").Publish(); + targetYawEntry = subTable->GetDoubleTopic("targetYaw").Publish(); + targetPoseEntry = subTable->GetDoubleArrayTopic("targetPose").Publish(); + targetSkewEntry = subTable->GetDoubleTopic("targetSkew").Publish(); + + bestTargetPosX = subTable->GetDoubleTopic("targetPixelsX").Publish(); + bestTargetPosY = subTable->GetDoubleTopic("targetPixelsY").Publish(); + + heartbeatTopic = subTable->GetIntegerTopic("heartbeat"); + heartbeatPublisher = heartbeatTopic.Publish(); + + cameraIntrinsicsPublisher = + subTable->GetDoubleArrayTopic("cameraIntrinsics").Publish(); + cameraDistortionPublisher = + subTable->GetDoubleArrayTopic("cameraDistortion").Publish(); + } + + private: +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h index e97f6c8d71..49f73ed53b 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h @@ -27,15 +27,15 @@ class PNPResult { public: // This could be wrapped in an std::optional, but chose to do it this way to // mirror Java - bool isPresent; + bool isPresent{false}; - frc::Transform3d best; - double bestReprojErr; + frc::Transform3d best{}; + double bestReprojErr{0}; - frc::Transform3d alt; - double altReprojErr; + frc::Transform3d alt{}; + double altReprojErr{0}; - double ambiguity; + double ambiguity{0}; bool operator==(const PNPResult& other) const; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.gitignore b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore new file mode 100644 index 0000000000..34878ab18c --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore @@ -0,0 +1 @@ +vendordeps diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000000..a6e62683de --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": true, + "currentLanguage": "cpp", + "projectYear": "2023", + "teamNumber": 5 +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md new file mode 100644 index 0000000000..3d5a824cad --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2021 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/build.gradle b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle new file mode 100644 index 0000000000..8eeef20126 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle @@ -0,0 +1,121 @@ +plugins { + id "cpp" + id "google-test-test-suite" + id "edu.wpi.first.GradleRIO" + + id "com.dorongold.task-tree" version "2.1.0" +} + +wpi.maven.useLocal = false +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = '2024.1.1-beta-3-53-g31cd015' +wpi.versions.wpimathVersion = '2024.1.1-beta-3-53-g31cd015' + +repositories { + mavenLocal() + jcenter() +} + +apply from: "${rootDir}/../shared/examples_common.gradle" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamOrDefault(5940) + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcCpp(getArtifactTypeClass('FRCNativeArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcCpp + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Set to true to run simulation in debug mode +wpi.cpp.debugSimulation = false + +// Default enable simgui +wpi.sim.addGui().defaultEnabled = true +// Enable DS but not by default +wpi.sim.addDriverstation() + +model { + components { + frcUserProgram(NativeExecutableSpec) { + // We don't need to build for roborio -- if we do, we need to install + // a roborio toolchain every time we build in CI + // Ideally, we'd be able to set the roborio toolchain as optional, but + // I can't figure out how to set that environment variable from build.gradle + // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71) + // for now, commented out + + // targetPlatform wpi.platforms.roborio + + if (includeDesktopSupport) { + targetPlatform wpi.platforms.desktop + } + + sources.cpp { + source { + srcDir 'src/main/cpp' + include '**/*.cpp', '**/*.cc' + } + exportedHeaders { + srcDir 'src/main/include' + } + } + + // Set deploy task to deploy this component + deployArtifact.component = it + + // Enable run tasks for this component + wpi.cpp.enableExternalTasks(it) + + // Enable simulation for this component + wpi.sim.enable(it) + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. + wpi.cpp.vendor.cpp(it) + wpi.cpp.deps.wpilib(it) + } + } + testSuites { + frcUserProgramTest(GoogleTestTestSuiteSpec) { + testing $.components.frcUserProgram + + sources.cpp { + source { + srcDir 'src/test/cpp' + include '**/*.cpp' + } + } + + // Enable run tasks for this component + wpi.cpp.enableExternalTasks(it) + + wpi.cpp.vendor.cpp(it) + wpi.cpp.deps.wpilib(it) + wpi.cpp.deps.googleTest(it) + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew b/photonlib-cpp-examples/swervedriveposeestsim/gradlew new file mode 100644 index 0000000000..0ef4c1e860 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/gradlew @@ -0,0 +1,241 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit + +APP_NAME="Gradle" +APP_BASE_NAME=${0##*/} + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + +# Collect all arguments for the java command; +# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of +# shell script including quotes and variable substitutions, so put them in +# double quotes to make sure that they get re-expanded; and +# * put everything else in single quotes, so that it's not re-expanded. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat b/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat new file mode 100644 index 0000000000..f127cfd49d --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat @@ -0,0 +1,91 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/photonlib-cpp-examples/swervedriveposeestsim/networktables.json b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json new file mode 100644 index 0000000000..fe51488c70 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json @@ -0,0 +1 @@ +[] diff --git a/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle new file mode 100644 index 0000000000..44fbca7512 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +rootProject.name = 'aimattarget' + +pluginManagement { + repositories { + mavenLocal() + jcenter() + gradlePluginPortal() + String frcYear = '2024' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json new file mode 100644 index 0000000000..c4b7efd3d8 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json @@ -0,0 +1,98 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json new file mode 100644 index 0000000000..1605887709 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json @@ -0,0 +1,64 @@ +{ + "Docking": { + "Data": [] + }, + "MainWindow": { + "GLOBAL": { + "fps": "120", + "height": "910", + "maximized": "0", + "style": "0", + "userScale": "2", + "width": "1550", + "xpos": "-1602", + "ypos": "79" + } + }, + "Window": { + "###/SmartDashboard/VisionSystemSim-main/Sim Field": { + "Collapsed": "0", + "Pos": "199,200", + "Size": "1342,628" + }, + "###FMS": { + "Collapsed": "0", + "Pos": "5,540", + "Size": "283,146" + }, + "###Joysticks": { + "Collapsed": "0", + "Pos": "359,95", + "Size": "796,240" + }, + "###NetworkTables": { + "Collapsed": "0", + "Pos": "865,52", + "Size": "830,620" + }, + "###Other Devices": { + "Collapsed": "0", + "Pos": "1025,20", + "Size": "250,695" + }, + "###System Joysticks": { + "Collapsed": "0", + "Pos": "5,350", + "Size": "192,218" + }, + "###Timing": { + "Collapsed": "0", + "Pos": "5,150", + "Size": "135,127" + }, + "Debug##Default": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "400,400" + }, + "Robot State": { + "Collapsed": "0", + "Pos": "5,20", + "Size": "92,99" + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json new file mode 100644 index 0000000000..e1ba9acce8 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json @@ -0,0 +1,57 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "width": 0.30000001192092896 + }, + "apriltag": { + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.6000000238418579 + }, + "bottom": 544, + "builtin": "2023 Charged Up", + "cameras": { + "arrowSize": 19, + "arrowWeight": 1.0, + "style": "Hidden" + }, + "height": 8.013679504394531, + "left": 46, + "right": 1088, + "top": 36, + "visibleTargetPoses": { + "image": "tag-blue.png" + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Drive": { + "open": true + }, + "open": true + }, + "photonvision": { + "open": true, + "photonvision": { + "open": true + } + } + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp new file mode 100644 index 0000000000..4505740955 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp @@ -0,0 +1,123 @@ +/* + * MIT License + * + * Copyright (c) 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 + +#include +#include + +void Robot::RobotInit() {} + +void Robot::RobotPeriodic() { + drivetrain.Periodic(); + + auto visionEst = vision.GetEstimatedGlobalPose(); + if (visionEst.has_value()) { + auto est = visionEst.value(); + auto estPose = est.estimatedPose.ToPose2d(); + auto estStdDevs = vision.GetEstimationStdDevs(estPose); + drivetrain.AddVisionMeasurement(est.estimatedPose.ToPose2d(), est.timestamp, + estStdDevs); + } + + drivetrain.Log(); +} + +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() { drivetrain.Stop(); } + +void Robot::DisabledExit() {} + +void Robot::AutonomousInit() { + autoTimer.Restart(); + frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; + drivetrain.ResetPose(pose, true); +} + +void Robot::AutonomousPeriodic() { + if (autoTimer.Get() < 10_s) { + drivetrain.Drive(0.5_mps, 0.5_mps, 0.5_rad_per_s, false); + } else { + autoTimer.Stop(); + drivetrain.Stop(); + } +} + +void Robot::AutonomousExit() {} + +void Robot::TeleopInit() {} + +void Robot::TeleopPeriodic() { + double forward = -controller.GetLeftY() * kDriveSpeed; + if (std::abs(forward) < 0.1) { + forward = 0; + } + forward = forwardLimiter.Calculate(forward); + + double strafe = -controller.GetLeftX() * kDriveSpeed; + if (std::abs(strafe) < 0.1) { + strafe = 0; + } + strafe = strafeLimiter.Calculate(strafe); + + double turn = -controller.GetRightX() * kDriveSpeed; + if (std::abs(turn) < 0.1) { + turn = 0; + } + turn = turnLimiter.Calculate(turn); + + drivetrain.Drive(forward * constants::Swerve::kMaxLinearSpeed, + strafe * constants::Swerve::kMaxLinearSpeed, + turn * constants::Swerve::kMaxAngularSpeed, true); +} + +void Robot::TeleopExit() {} + +void Robot::TestInit() {} + +void Robot::TestPeriodic() {} + +void Robot::TestExit() {} + +void Robot::SimulationPeriodic() { + drivetrain.SimulationPeriodic(); + vision.SimPeriodic(drivetrain.GetSimPose()); + + frc::Field2d& debugField = vision.GetSimDebugField(); + debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose()); + debugField.GetObject("EstimatedRobotModules") + ->SetPoses(drivetrain.GetModulePoses()); + + units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); + units::volt_t loadedBattVolts = + frc::sim::BatterySim::Calculate({totalCurrent}); + frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts); +} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp new file mode 100644 index 0000000000..bf33d7189d --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp @@ -0,0 +1,211 @@ +/* + * MIT License + * + * Copyright (c) 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 "subsystems/SwerveDrive.h" + +#include + +#include +#include + +SwerveDrive::SwerveDrive() + : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), + frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), + gyroSim(gyro), + swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kDriveGearRatio, + constants::Swerve::kWheelDiameter / 2, + constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kSteerGearRatio, kinematics) {} + +void SwerveDrive::Periodic() { + for (auto& currentModule : swerveMods) { + currentModule.Periodic(); + } + + poseEstimator.Update(GetGyroYaw(), GetModulePositions()); +} + +void SwerveDrive::Drive(units::meters_per_second_t vx, + units::meters_per_second_t vy, + units::radians_per_second_t omega, bool openLoop) { + frc::ChassisSpeeds newChassisSpeeds = + frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); + SetChassisSpeeds(newChassisSpeeds, openLoop, false); +} + +void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, + bool openLoop, bool steerInPlace) { + SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), openLoop, + steerInPlace); + this->targetChassisSpeeds = newChassisSpeeds; +} + +void SwerveDrive::SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace) { + std::array desaturatedStates = desiredStates; + frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( + static_cast*>(&desaturatedStates), + constants::Swerve::kMaxLinearSpeed); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace); + } +} + +void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s, true); } + +void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp) { + poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp); +} + +void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp, + const Eigen::Vector3d& stdDevs) { + wpi::array newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)}; + poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs); +} + +void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { + if (resetSimPose) { + swerveDriveSim.Reset(pose, false); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A); + } + gyroSim.SetAngle(-pose.Rotation().Degrees()); + gyroSim.SetRate(0_rad_per_s); + } + + poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); +} + +frc::Pose2d SwerveDrive::GetPose() const { + return poseEstimator.GetEstimatedPosition(); +} + +frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); } + +frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); } + +frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { + return kinematics.ToChassisSpeeds(GetModuleStates()); +} + +std::array SwerveDrive::GetModuleStates() const { + std::array moduleStates; + moduleStates[0] = swerveMods[0].GetState(); + moduleStates[1] = swerveMods[1].GetState(); + moduleStates[2] = swerveMods[2].GetState(); + moduleStates[3] = swerveMods[3].GetState(); + return moduleStates; +} + +std::array SwerveDrive::GetModulePositions() + const { + std::array modulePositions; + modulePositions[0] = swerveMods[0].GetPosition(); + modulePositions[1] = swerveMods[1].GetPosition(); + modulePositions[2] = swerveMods[2].GetPosition(); + modulePositions[3] = swerveMods[3].GetPosition(); + return modulePositions; +} + +std::array SwerveDrive::GetModulePoses() const { + std::array modulePoses; + for (int i = 0; i < swerveMods.size(); i++) { + const SwerveModule& module = swerveMods[i]; + modulePoses[i] = GetPose().TransformBy(frc::Transform2d{ + module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()}); + } + return modulePoses; +} + +void SwerveDrive::Log() { + std::string table = "Drive/"; + frc::Pose2d pose = GetPose(); + frc::SmartDashboard::PutNumber(table + "X", pose.X().to()); + frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); + frc::SmartDashboard::PutNumber(table + "Heading", + pose.Rotation().Degrees().to()); + frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); + frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Omega Degrees", + chassisSpeeds.omega.convert().to()); + frc::SmartDashboard::PutNumber(table + "Target VX", + targetChassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "Target VY", + targetChassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Target Omega Degrees", + targetChassisSpeeds.omega.convert() + .to()); + + for (auto& module : swerveMods) { + module.Log(); + } +} + +void SwerveDrive::SimulationPeriodic() { + std::array driveInputs; + std::array steerInputs; + for (int i = 0; i < swerveMods.size(); i++) { + driveInputs[i] = swerveMods[i].GetDriveVoltage(); + steerInputs[i] = swerveMods[i].GetSteerVoltage(); + } + swerveDriveSim.SetDriveInputs(driveInputs); + swerveDriveSim.SetSteerInputs(steerInputs); + + swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod); + + auto driveStates = swerveDriveSim.GetDriveStates(); + auto steerStates = swerveDriveSim.GetSteerStates(); + totalCurrentDraw = 0_A; + std::array driveCurrents = + swerveDriveSim.GetDriveCurrentDraw(); + for (const auto& current : driveCurrents) { + totalCurrentDraw += current; + } + std::array steerCurrents = + swerveDriveSim.GetSteerCurrentDraw(); + for (const auto& current : steerCurrents) { + totalCurrentDraw += current; + } + for (int i = 0; i < swerveMods.size(); i++) { + units::meter_t drivePos{driveStates[i](0, 0)}; + units::meters_per_second_t driveRate{driveStates[i](1, 0)}; + units::radian_t steerPos{steerStates[i](0, 0)}; + units::radians_per_second_t steerRate{steerStates[i](1, 0)}; + swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], + steerPos, steerRate, steerCurrents[i]); + } + gyroSim.SetRate(-swerveDriveSim.GetOmega()); + gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); +} + +frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } + +units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp new file mode 100644 index 0000000000..31bbe09e61 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -0,0 +1,285 @@ +/* + * MIT License + * + * Copyright (c) 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 "subsystems/SwerveDriveSim.h" + +#include + +#include +#include + +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +SwerveDriveSim::SwerveDriveSim( + const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : SwerveDriveSim( + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -driveFF.kV.to() / driveFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / driveFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + driveFF.kS, driveMotor, driveGearing, driveWheelRadius, + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -steerFF.kV.to() / steerFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / steerFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + steerFF.kS, steerMotor, steerGearing, kinematics) {} + +SwerveDriveSim::SwerveDriveSim( + const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : drivePlant(drivePlant), + driveKs(driveKs), + driveMotor(driveMotor), + driveGearing(driveGearing), + driveWheelRadius(driveWheelRadius), + steerPlant(steerPlant), + steerKs(steerKs), + steerMotor(steerMotor), + steerGearing(steerGearing), + kinematics(kinematics) {} + +void SwerveDriveSim::SetDriveInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < driveInputs.size(); i++) { + units::volt_t input = inputs[i]; + driveInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +void SwerveDriveSim::SetSteerInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < steerInputs.size(); i++) { + units::volt_t input = inputs[i]; + steerInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +Eigen::Matrix SwerveDriveSim::CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS) { + auto Ax = discA * x; + double nextStateVel = Ax(1, 0); + double inputToStop = nextStateVel / -discB(1, 0); + double ksSystemEffect = + std::clamp(inputToStop, -kS.to(), kS.to()); + + nextStateVel += discB(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB(1, 0); + double signToStop = sgn(inputToStop); + double inputSign = sgn(input.to()); + double ksInputEffect = 0; + + if (std::abs(ksSystemEffect) < kS.to()) { + double absInput = std::abs(input.to()); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } else if ((input.to() * signToStop) > (inputToStop * signToStop)) { + double absInput = std::abs(input.to() - inputToStop); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } + + auto sF = Eigen::Matrix{input.to() + ksSystemEffect + + ksInputEffect}; + auto Bu = discB * sF; + auto retVal = Ax + Bu; + return retVal; +} + +void SwerveDriveSim::Update(units::second_t dt) { + Eigen::Matrix driveDiscA; + Eigen::Matrix driveDiscB; + frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, + &driveDiscB); + + Eigen::Matrix steerDiscA; + Eigen::Matrix steerDiscB; + frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, + &steerDiscB); + + std::array moduleDeltas; + + for (int i = 0; i < numModules; i++) { + double prevDriveStatePos = driveStates[i](0, 0); + driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i], + driveInputs[i], driveKs); + double currentDriveStatePos = driveStates[i](0, 0); + steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i], + steerInputs[i], steerKs); + double currentSteerStatePos = steerStates[i](0, 0); + moduleDeltas[i] = frc::SwerveModulePosition{ + units::meter_t{currentDriveStatePos - prevDriveStatePos}, + frc::Rotation2d{units::radian_t{currentSteerStatePos}}}; + } + + frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); + pose = pose.Exp(twist); + omega = twist.dtheta / dt; +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { + this->pose = pose; + if (!preserveMotion) { + for (int i = 0; i < numModules; i++) { + driveStates[i] = Eigen::Matrix{0, 0}; + steerStates[i] = Eigen::Matrix{0, 0}; + } + omega = 0_rad_per_s; + } +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, + const std::array, + numModules>& moduleDriveStates, + const std::array, + numModules>& moduleSteerStates) { + this->pose = pose; + driveStates = moduleDriveStates; + steerStates = moduleSteerStates; + omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega; +} + +frc::Pose2d SwerveDriveSim::GetPose() const { return pose; } + +std::array +SwerveDriveSim::GetModulePositions() const { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev, + units::radian_t steerStdDev) { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)} + + randDist(generator) * driveStdDev, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} + + randDist(generator) * steerStdDev}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetModuleStates() { + std::array states; + for (int i = 0; i < numModules; i++) { + states[i] = frc::SwerveModuleState{ + units::meters_per_second_t{driveStates[i](1, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return states; +} + +std::array, numModules> +SwerveDriveSim::GetDriveStates() const { + return driveStates; +} + +std::array, numModules> +SwerveDriveSim::GetSteerStates() const { + return steerStates; +} + +units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } + +units::ampere_t SwerveDriveSim::GetCurrentDraw( + const frc::DCMotor& motor, units::radians_per_second_t velocity, + units::volt_t inputVolts, units::volt_t batteryVolts) const { + units::volt_t effVolts = inputVolts - velocity / motor.Kv; + if (inputVolts >= 0_V) { + effVolts = std::clamp(effVolts, 0_V, inputVolts); + } else { + effVolts = std::clamp(effVolts, inputVolts, 0_V); + } + auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R); + return retVal; +} + +std::array SwerveDriveSim::GetDriveCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / + driveWheelRadius.to(); + currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i], + frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +std::array SwerveDriveSim::GetSteerCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; + // TODO: If uncommented we get huge current values.. Not sure how to fix + // atm. :( + currents[i] = 20_A; + // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i], + // frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { + units::ampere_t total{0}; + for (const auto& val : GetDriveCurrentDraw()) { + total += val; + } + for (const auto& val : GetSteerCurrentDraw()) { + total += val; + } + return total; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp new file mode 100644 index 0000000000..781c28a338 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp @@ -0,0 +1,146 @@ +/* + * MIT License + * + * Copyright (c) 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 "subsystems/SwerveModule.h" + +#include + +#include +#include +#include + +SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) + : moduleConstants(consts), + driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}), + driveEncoder(frc::Encoder{moduleConstants.driveEncoderA, + moduleConstants.driveEncoderB}), + steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}), + steerEncoder(frc::Encoder{moduleConstants.steerEncoderA, + moduleConstants.steerEncoderB}), + driveEncoderSim(driveEncoder), + steerEncoderSim(steerEncoder) { + driveEncoder.SetDistancePerPulse( + constants::Swerve::kDriveDistPerPulse.to()); + steerEncoder.SetDistancePerPulse( + constants::Swerve::kSteerRadPerPulse.to()); + steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi); +} + +void SwerveModule::Periodic() { + units::volt_t steerPID = units::volt_t{ + steerPIDController.Calculate(GetAbsoluteHeading().Radians().to(), + desiredState.angle.Radians().to())}; + steerMotor.SetVoltage(steerPID); + + units::volt_t driveFF = + constants::Swerve::kDriveFF.Calculate(desiredState.speed); + units::volt_t drivePID{0}; + if (!openLoop) { + drivePID = units::volt_t{drivePIDController.Calculate( + driveEncoder.GetRate(), desiredState.speed.to())}; + } + driveMotor.SetVoltage(driveFF + drivePID); +} + +void SwerveModule::SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace) { + frc::Rotation2d currentRotation = GetAbsoluteHeading(); + frc::SwerveModuleState optimizedState = + frc::SwerveModuleState::Optimize(newState, currentRotation); + desiredState = optimizedState; +} + +frc::Rotation2d SwerveModule::GetAbsoluteHeading() const { + return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}}; +} + +frc::SwerveModuleState SwerveModule::GetState() const { + return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps, + GetAbsoluteHeading()}; +} + +frc::SwerveModulePosition SwerveModule::GetPosition() const { + return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, + GetAbsoluteHeading()}; +} + +units::volt_t SwerveModule::GetDriveVoltage() const { + return driveMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::volt_t SwerveModule::GetSteerVoltage() const { + return steerMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::ampere_t SwerveModule::GetDriveCurrentSim() const { + return driveCurrentSim; +} + +units::ampere_t SwerveModule::GetSteerCurrentSim() const { + return steerCurrentSim; +} + +constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const { + return moduleConstants; +} + +void SwerveModule::Log() { + frc::SwerveModuleState state = GetState(); + + std::string table = + "Module " + std::to_string(moduleConstants.moduleNum) + "/"; + frc::SmartDashboard::PutNumber(table + "Steer Degrees", + frc::AngleModulus(state.angle.Radians()) + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Steer Target Degrees", + units::radian_t{steerPIDController.GetSetpoint()} + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Feet", + state.speed.convert().to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Target Feet", + desiredState.speed.convert().to()); + frc::SmartDashboard::PutNumber(table + "Drive Current", + driveCurrentSim.to()); + frc::SmartDashboard::PutNumber(table + "Steer Current", + steerCurrentSim.to()); +} + +void SwerveModule::SimulationUpdate( + units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent) { + driveEncoderSim.SetDistance(driveEncoderDist.to()); + driveEncoderSim.SetRate(driveEncoderRate.to()); + driveCurrentSim = driveCurrent; + steerEncoderSim.SetDistance(steerEncoderDist.to()); + steerEncoderSim.SetRate(steerEncoderRate.to()); + steerCurrentSim = steerCurrent; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt b/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt new file mode 100644 index 0000000000..15bc5c1ebe --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt @@ -0,0 +1,4 @@ +Files placed in this directory will be deployed to the RoboRIO into the + 'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory' + function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy + directory. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h new file mode 100644 index 0000000000..70be37d406 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h @@ -0,0 +1,118 @@ +/* + * MIT License + * + * Copyright (c) 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 + +namespace constants { +namespace Vision { +static constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; +static const frc::Transform3d kRobotToCam{ + frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, + frc::Rotation3d{0_rad, 0_rad, 0_rad}}; +static const frc::AprilTagFieldLayout kTagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)}; + +static const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; +static const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; +} // namespace Vision +namespace Swerve { + +static constexpr units::meter_t kTrackWidth{18.5_in}; +static constexpr units::meter_t kTrackLength{18.5_in}; +static constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2}; +static constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2}; +static constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; +static constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +static constexpr units::meter_t kWheelDiameter{4_in}; +static constexpr units::meter_t kWheelCircumference{kWheelDiameter * + std::numbers::pi}; + +static constexpr double kDriveGearRatio = 6.75; +static constexpr double kSteerGearRatio = 12.8; + +static constexpr units::meter_t kDriveDistPerPulse = + kWheelCircumference / 1024.0 / kDriveGearRatio; +static constexpr units::radian_t kSteerRadPerPulse = + units::radian_t{2 * std::numbers::pi} / 1024.0; + +static constexpr double kDriveKP = 1.0; +static constexpr double kDriveKI = 0.0; +static constexpr double kDriveKD = 0.0; + +static constexpr double kSteerKP = 20.0; +static constexpr double kSteerKI = 0.0; +static constexpr double kSteerKD = 0.25; + +static const frc::SimpleMotorFeedforward kDriveFF{ + 0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq}; + +static const frc::SimpleMotorFeedforward kSteerFF{ + 0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; + +struct ModuleConstants { + public: + const int moduleNum; + const int driveMotorId; + const int driveEncoderA; + const int driveEncoderB; + const int steerMotorId; + const int steerEncoderA; + const int steerEncoderB; + const double angleOffset; + const frc::Translation2d centerOffset; + + ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, + int driveEncoderB, int steerMotorId, int steerEncoderA, + int steerEncoderB, double angleOffset, units::meter_t xOffset, + units::meter_t yOffset) + : moduleNum(moduleNum), + driveMotorId(driveMotorId), + driveEncoderA(driveEncoderA), + driveEncoderB(driveEncoderB), + steerMotorId(steerMotorId), + steerEncoderA(steerEncoderA), + steerEncoderB(steerEncoderB), + angleOffset(angleOffset), + centerOffset(frc::Translation2d{xOffset, yOffset}) {} +}; + +static const ModuleConstants FL_CONSTANTS{ + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2}; +static const ModuleConstants FR_CONSTANTS{ + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2}; +static const ModuleConstants BL_CONSTANTS{ + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2}; +static const ModuleConstants BR_CONSTANTS{ + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2}; +} // namespace Swerve +} // namespace constants diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h new file mode 100644 index 0000000000..7a3697494a --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h @@ -0,0 +1,62 @@ +/* + * MIT License + * + * Copyright (c) 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 "Vision.h" +#include "subsystems/SwerveDrive.h" + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void DisabledExit() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void AutonomousExit() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TeleopExit() override; + void TestInit() override; + void TestPeriodic() override; + void TestExit() override; + void SimulationPeriodic() override; + + private: + SwerveDrive drivetrain{}; + Vision vision{}; + frc::XboxController controller{0}; + frc::SlewRateLimiter forwardLimiter{1.0 / 0.6_s}; + frc::SlewRateLimiter strafeLimiter{1.0 / 0.6_s}; + frc::SlewRateLimiter turnLimiter{1.0 / 0.33_s}; + frc::Timer autoTimer{}; + double kDriveSpeed{0.6}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h new file mode 100644 index 0000000000..619b34ade7 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h @@ -0,0 +1,152 @@ +/* + * MIT License + * + * Copyright (c) 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 +#include + +#include "Constants.h" + +class Vision { + public: + Vision() { + photonEstimator.SetMultiTagFallbackStrategy( + photon::PoseStrategy::LOWEST_AMBIGUITY); + + if (frc::RobotBase::IsSimulation()) { + visionSim = std::make_unique("main"); + + visionSim->AddAprilTags(constants::Vision::kTagLayout); + + cameraProp = std::make_unique(); + + cameraProp->SetCalibration(960, 720, frc::Rotation2d{90_deg}); + cameraProp->SetCalibError(.35, .10); + cameraProp->SetFPS(15_Hz); + cameraProp->SetAvgLatency(50_ms); + cameraProp->SetLatencyStdDev(15_ms); + + cameraSim = std::make_shared(camera.get(), + *cameraProp.get()); + + visionSim->AddCamera(cameraSim.get(), robotToCam); + cameraSim->EnableDrawWireframe(true); + } + } + + photon::PhotonPipelineResult GetLatestResult() { + return camera->GetLatestResult(); + } + + std::optional GetEstimatedGlobalPose() { + auto visionEst = photonEstimator.Update(); + units::second_t latestTimestamp = camera->GetLatestResult().GetTimestamp(); + bool newResult = + units::math::abs(latestTimestamp - lastEstTimestamp) > 1e-5_s; + if (frc::RobotBase::IsSimulation()) { + if (visionEst.has_value()) { + GetSimDebugField() + .GetObject("VisionEstimation") + ->SetPose(visionEst.value().estimatedPose.ToPose2d()); + } else { + if (newResult) { + GetSimDebugField().GetObject("VisionEstimation")->SetPoses({}); + } + } + } + if (newResult) { + lastEstTimestamp = latestTimestamp; + } + return visionEst; + } + + Eigen::Matrix GetEstimationStdDevs(frc::Pose2d estimatedPose) { + Eigen::Matrix estStdDevs = + constants::Vision::kSingleTagStdDevs; + auto targets = GetLatestResult().GetTargets(); + int numTags = 0; + units::meter_t avgDist = 0_m; + for (const auto& tgt : targets) { + auto tagPose = + photonEstimator.GetFieldLayout().GetTagPose(tgt.GetFiducialId()); + if (tagPose.has_value()) { + numTags++; + avgDist += tagPose.value().ToPose2d().Translation().Distance( + estimatedPose.Translation()); + } + } + if (numTags == 0) { + return estStdDevs; + } + avgDist /= numTags; + if (numTags > 1) { + estStdDevs = constants::Vision::kMultiTagStdDevs; + } + if (numTags == 1 && avgDist > 4_m) { + estStdDevs = (Eigen::MatrixXd(3, 1) << std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()) + .finished(); + } else { + estStdDevs = estStdDevs * (1 + (avgDist.value() * avgDist.value() / 30)); + } + return estStdDevs; + } + + void SimPeriodic(frc::Pose2d robotSimPose) { + visionSim->Update(robotSimPose); + } + + void ResetSimPose(frc::Pose2d pose) { + if (frc::RobotBase::IsSimulation()) { + visionSim->ResetRobotPose(pose); + } + } + + frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } + + private: + frc::Transform3d robotToCam{frc::Translation3d{0.5_m, 0.5_m, 0.5_m}, + frc::Rotation3d{}}; + photon::PhotonPoseEstimator photonEstimator{ + LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp), + photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR, + photon::PhotonCamera{"photonvision"}, robotToCam}; + std::shared_ptr camera{photonEstimator.GetCamera()}; + std::unique_ptr visionSim; + std::unique_ptr cameraProp; + std::shared_ptr cameraSim; + units::second_t lastEstTimestamp{0_s}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h new file mode 100644 index 0000000000..1e3d26b32f --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h @@ -0,0 +1,85 @@ +/* + * MIT License + * + * Copyright (c) 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 "SwerveDriveSim.h" +#include "SwerveModule.h" + +class SwerveDrive { + public: + SwerveDrive(); + void Periodic(); + void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, + units::radians_per_second_t omega, bool openLoop); + void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, + bool openLoop, bool steerInPlace); + void SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace); + void Stop(); + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp); + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp, + const Eigen::Vector3d& stdDevs); + void ResetPose(const frc::Pose2d& pose, bool resetSimPose); + frc::Pose2d GetPose() const; + frc::Rotation2d GetHeading() const; + frc::Rotation2d GetGyroYaw() const; + frc::ChassisSpeeds GetChassisSpeeds() const; + std::array GetModuleStates() const; + std::array GetModulePositions() const; + std::array GetModulePoses() const; + void Log(); + void SimulationPeriodic(); + frc::Pose2d GetSimPose() const; + units::ampere_t GetCurrentDraw() const; + + private: + std::array swerveMods{ + SwerveModule{constants::Swerve::FL_CONSTANTS}, + SwerveModule{constants::Swerve::FR_CONSTANTS}, + SwerveModule{constants::Swerve::BL_CONSTANTS}, + SwerveModule{constants::Swerve::BR_CONSTANTS}}; + frc::SwerveDriveKinematics<4> kinematics{ + swerveMods[0].GetModuleConstants().centerOffset, + swerveMods[1].GetModuleConstants().centerOffset, + swerveMods[2].GetModuleConstants().centerOffset, + swerveMods[3].GetModuleConstants().centerOffset, + }; + frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0}; + frc::SwerveDrivePoseEstimator<4> poseEstimator; + frc::ChassisSpeeds targetChassisSpeeds{}; + + frc::sim::ADXRS450_GyroSim gyroSim; + SwerveDriveSim swerveDriveSim; + units::ampere_t totalCurrentDraw{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h new file mode 100644 index 0000000000..c1cee3e6f1 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h @@ -0,0 +1,102 @@ +/* + * MIT License + * + * Copyright (c) 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 + +static constexpr int numModules{4}; + +class SwerveDriveSim { + public: + SwerveDriveSim(const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant, + units::volt_t driveKs, const frc::DCMotor& driveMotor, + double driveGearing, units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, + units::volt_t steerKs, const frc::DCMotor& steerMotor, + double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + void SetDriveInputs(const std::array& inputs); + void SetSteerInputs(const std::array& inputs); + static Eigen::Matrix CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS); + void Update(units::second_t dt); + void Reset(const frc::Pose2d& pose, bool preserveMotion); + void Reset(const frc::Pose2d& pose, + const std::array, numModules>& + moduleDriveStates, + const std::array, numModules>& + moduleSteerStates); + frc::Pose2d GetPose() const; + std::array GetModulePositions() const; + std::array GetNoisyModulePositions( + units::meter_t driveStdDev, units::radian_t steerStdDev); + std::array GetModuleStates(); + std::array, numModules> GetDriveStates() const; + std::array, numModules> GetSteerStates() const; + units::radians_per_second_t GetOmega() const; + units::ampere_t GetCurrentDraw(const frc::DCMotor& motor, + units::radians_per_second_t velocity, + units::volt_t inputVolts, + units::volt_t batteryVolts) const; + std::array GetDriveCurrentDraw() const; + std::array GetSteerCurrentDraw() const; + units::ampere_t GetTotalCurrentDraw() const; + + private: + std::random_device rd{}; + std::mt19937 generator{rd()}; + std::normal_distribution randDist{0.0, 1.0}; + const frc::LinearSystem<2, 1, 2> drivePlant; + const units::volt_t driveKs; + const frc::DCMotor driveMotor; + const double driveGearing; + const units::meter_t driveWheelRadius; + const frc::LinearSystem<2, 1, 2> steerPlant; + const units::volt_t steerKs; + const frc::DCMotor steerMotor; + const double steerGearing; + const frc::SwerveDriveKinematics kinematics; + std::array driveInputs{}; + std::array, numModules> driveStates{}; + std::array steerInputs{}; + std::array, numModules> steerStates{}; + frc::Pose2d pose{frc::Pose2d{}}; + units::radians_per_second_t omega{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h new file mode 100644 index 0000000000..444e4bdf44 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h @@ -0,0 +1,81 @@ +/* + * MIT License + * + * Copyright (c) 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 "Constants.h" + +class SwerveModule { + public: + explicit SwerveModule(const constants::Swerve::ModuleConstants& consts); + void Periodic(); + void SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace); + frc::Rotation2d GetAbsoluteHeading() const; + frc::SwerveModuleState GetState() const; + frc::SwerveModulePosition GetPosition() const; + units::volt_t GetDriveVoltage() const; + units::volt_t GetSteerVoltage() const; + units::ampere_t GetDriveCurrentSim() const; + units::ampere_t GetSteerCurrentSim() const; + constants::Swerve::ModuleConstants GetModuleConstants() const; + void Log(); + void SimulationUpdate(units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, + units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent); + + private: + const constants::Swerve::ModuleConstants moduleConstants; + + frc::PWMSparkMax driveMotor; + frc::Encoder driveEncoder; + frc::PWMSparkMax steerMotor; + frc::Encoder steerEncoder; + + frc::SwerveModuleState desiredState{}; + bool openLoop{false}; + + frc::PIDController drivePIDController{constants::Swerve::kDriveKP, + constants::Swerve::kDriveKI, + constants::Swerve::kDriveKD}; + frc::PIDController steerPIDController{constants::Swerve::kSteerKP, + constants::Swerve::kSteerKI, + constants::Swerve::kSteerKD}; + + frc::sim::EncoderSim driveEncoderSim; + units::ampere_t driveCurrentSim{0}; + frc::sim::EncoderSim steerEncoderSim; + units::ampere_t steerCurrentSim{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp new file mode 100644 index 0000000000..031d1ce96b --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp @@ -0,0 +1,34 @@ +/* + * MIT License + * + * Copyright (c) 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 "gtest/gtest.h" + +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png b/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png new file mode 100644 index 0000000000..25990c8399 Binary files /dev/null and b/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png differ diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png new file mode 100644 index 0000000000..04b9e4f7b7 Binary files /dev/null and b/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png differ diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png new file mode 100644 index 0000000000..63029fcf20 Binary files /dev/null and b/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png differ diff --git a/shared/javacpp/setupBuild.gradle b/shared/javacpp/setupBuild.gradle index 66106c6057..dd8a5bec32 100644 --- a/shared/javacpp/setupBuild.gradle +++ b/shared/javacpp/setupBuild.gradle @@ -88,6 +88,8 @@ model { } } + nativeUtils.useRequiredLibrary(it, "cscore_shared") + nativeUtils.useRequiredLibrary(it, "cameraserver_shared") nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") nativeUtils.useRequiredLibrary(it, "googletest_static") nativeUtils.useRequiredLibrary(it, "apriltag_shared")