mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
Update C++ Simulation to match Java (#1026)
This commit is contained in:
Drew Williams
@@ -26,6 +26,7 @@
|
||||
|
||||
#include <frc/Errors.h>
|
||||
#include <frc/Timer.h>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/core/mat.hpp>
|
||||
|
||||
#include "PhotonVersion.h"
|
||||
@@ -128,8 +129,13 @@ LEDMode PhotonCamera::GetLEDMode() const {
|
||||
std::optional<cv::Mat> 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<double>(i, j) = camCoeffs[(j * 3) + i];
|
||||
}
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
@@ -145,8 +151,11 @@ const std::string_view PhotonCamera::GetCameraName() const {
|
||||
std::optional<cv::Mat> 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<double>(i, 0) = distCoeffs[i];
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
@@ -39,11 +39,7 @@
|
||||
#include <units/time.h>
|
||||
#include <wpi/deprecated.h>
|
||||
|
||||
#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<nt::NetworkTable> GetCameraTable() const { return rootTable; }
|
||||
|
||||
// For use in tests
|
||||
bool test = false;
|
||||
PhotonPipelineResult testResult;
|
||||
|
||||
@@ -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 <cameraserver/CameraServer.h>
|
||||
#include <photon/PhotonCamera.h>
|
||||
#include <photon/PhotonTargetSortMode.h>
|
||||
#include <photon/estimation/CameraTargetRelation.h>
|
||||
#include <photon/estimation/VisionEstimation.h>
|
||||
#include <photon/networktables/NTTopicSet.h>
|
||||
#include <photon/simulation/SimCameraProperties.h>
|
||||
#include <photon/simulation/VideoSimUtil.h>
|
||||
#include <photon/simulation/VisionTargetSim.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <units/math.h>
|
||||
#include <wpi/timestamp.h>
|
||||
|
||||
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<cv::Point2f>& points) {
|
||||
for (const auto& pt : points) {
|
||||
if (std::clamp<float>(pt.x, 0, prop.GetResWidth()) != pt.x ||
|
||||
std::clamp<float>(pt.y, 0, prop.GetResHeight()) != pt.y) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
std::optional<uint64_t> ConsumeNextEntryTime() {
|
||||
uint64_t now = wpi::Now();
|
||||
uint64_t timestamp{};
|
||||
int iter = 0;
|
||||
while (now >= nextNTEntryTime) {
|
||||
timestamp = nextNTEntryTime;
|
||||
uint64_t frameTime = prop.EstSecUntilNextFrame()
|
||||
.convert<units::microseconds>()
|
||||
.to<uint64_t>();
|
||||
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<VisionTargetSim> 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<std::pair<VisionTargetSim, std::vector<cv::Point2f>>>
|
||||
visibleTgts{};
|
||||
std::vector<PhotonTrackedTarget> 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<frc::Translation3d> fieldCorners = tgt.GetFieldVertices();
|
||||
if (tgt.GetModel().GetIsSpherical()) {
|
||||
TargetModel model = tgt.GetModel();
|
||||
fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose(
|
||||
tgt.GetPose().Translation(), cameraPose.Translation()));
|
||||
}
|
||||
|
||||
std::vector<cv::Point2f> 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<double, 4> 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<units::degrees>().to<float>()};
|
||||
std::vector<cv::Point2f> 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<cv::Point2f> noisyTargetCorners =
|
||||
prop.EstPixelNoise(imagePoints);
|
||||
cv::RotatedRect minAreaRect =
|
||||
OpenCVHelp::GetMinAreaRect(noisyTargetCorners);
|
||||
std::vector<cv::Point2f> 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<std::pair<float, float>> tempCorners =
|
||||
OpenCVHelp::PointsToCorners(minAreaRectPts);
|
||||
wpi::SmallVector<std::pair<double, double>, 4> smallVec;
|
||||
|
||||
for (const auto& corner : tempCorners) {
|
||||
smallVec.emplace_back(
|
||||
std::make_pair(static_cast<double>(corner.first),
|
||||
static_cast<double>(corner.second)));
|
||||
}
|
||||
|
||||
std::vector<std::pair<float, float>> cornersFloat =
|
||||
OpenCVHelp::PointsToCorners(noisyTargetCorners);
|
||||
|
||||
std::vector<std::pair<double, double>> cornersDouble{cornersFloat.begin(),
|
||||
cornersFloat.end()};
|
||||
detectableTgts.emplace_back(PhotonTrackedTarget{
|
||||
centerRot.Z().convert<units::degrees>().to<double>(),
|
||||
-centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
|
||||
centerRot.X().convert<units::degrees>().to<double>(), 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<cv::Point2f> corners = pair.second;
|
||||
|
||||
if (tgt.fiducialId > 0) {
|
||||
VideoSimUtil::Warp165h5TagImage(tgt.fiducialId, corners, true,
|
||||
videoSimFrameRaw);
|
||||
} else if (!tgt.GetModel().GetIsSpherical()) {
|
||||
std::vector<cv::Point2f> 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<int>(
|
||||
VideoSimUtil::GetScaledThickness(15, videoSimFrameProcessed)),
|
||||
static_cast<int>(
|
||||
VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
|
||||
cv::LINE_AA);
|
||||
for (const auto& tgt : detectableTgts) {
|
||||
auto detectedCornersDouble = tgt.GetDetectedCorners();
|
||||
std::vector<std::pair<float, float>> 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<int>(VideoSimUtil::GetScaledThickness(
|
||||
1, videoSimFrameProcessed)),
|
||||
cv::LINE_AA);
|
||||
|
||||
wpi::SmallVector<std::pair<double, double>, 4> smallVec =
|
||||
tgt.GetMinAreaRectCorners();
|
||||
|
||||
std::vector<std::pair<float, float>> cornersCopy{};
|
||||
cornersCopy.reserve(4);
|
||||
|
||||
for (const auto& corner : smallVec) {
|
||||
cornersCopy.emplace_back(
|
||||
std::make_pair(static_cast<float>(corner.first),
|
||||
static_cast<float>(corner.second)));
|
||||
}
|
||||
|
||||
VideoSimUtil::DrawPoly(
|
||||
OpenCVHelp::CornersToPoints(cornersCopy),
|
||||
static_cast<int>(
|
||||
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<frc::AprilTag> visibleLayoutTags =
|
||||
VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
|
||||
if (visibleLayoutTags.size() > 1) {
|
||||
wpi::SmallVector<int16_t, 32> 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<units::milliseconds>().to<double>(),
|
||||
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<double, 3> 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<double, 4> poseData{
|
||||
transform.X().to<double>(), transform.Y().to<double>(),
|
||||
transform.Rotation().ToRotation2d().Degrees().to<double>()};
|
||||
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
|
||||
}
|
||||
|
||||
auto intrinsics = prop.GetIntrinsics();
|
||||
std::vector<double> intrinsicsView{intrinsics.data(),
|
||||
intrinsics.data() + intrinsics.size()};
|
||||
ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp);
|
||||
|
||||
auto distortion = prop.GetDistCoeffs();
|
||||
std::vector<double> 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<double>::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
|
||||
@@ -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 <photon/estimation/OpenCVHelp.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <random>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <frc/Errors.h>
|
||||
#include <frc/MathUtil.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/geometry/Translation3d.h>
|
||||
#include <units/frequency.h>
|
||||
#include <units/time.h>
|
||||
|
||||
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<units::degree_t>(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<double, 5, 1> 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<double, 3, 3> 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<double, 3, 3>& newCamIntrinsics,
|
||||
const Eigen::Matrix<double, 5, 1>& newDistCoeffs) {
|
||||
resWidth = width;
|
||||
resHeight = height;
|
||||
camIntrinsics = newCamIntrinsics;
|
||||
distCoeffs = newDistCoeffs;
|
||||
|
||||
std::array<frc::Translation3d, 4> 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<double, 3, 1>{
|
||||
p[i].X().to<double>(), p[i].Y().to<double>(), p[i].Z().to<double>()});
|
||||
}
|
||||
}
|
||||
|
||||
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<double>(resWidth) / static_cast<double>(resHeight);
|
||||
}
|
||||
|
||||
Eigen::Matrix<double, 3, 3> GetIntrinsics() const { return camIntrinsics; }
|
||||
|
||||
Eigen::Matrix<double, 5, 1> 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<cv::Point2f>& 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<double>, std::optional<double>> 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<double, 3, 1> av{relA.X().to<double>(), relA.Y().to<double>(),
|
||||
relA.Z().to<double>()};
|
||||
Eigen::Matrix<double, 3, 1> bv{relB.X().to<double>(), relB.Y().to<double>(),
|
||||
relB.Z().to<double>()};
|
||||
Eigen::Matrix<double, 3, 1> abv = bv - av;
|
||||
|
||||
bool aVisible = true;
|
||||
bool bVisible = true;
|
||||
for (size_t i = 0; i < viewplanes.size(); i++) {
|
||||
Eigen::Matrix<double, 3, 1> 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<double, 4> intersections{std::nan(""), std::nan(""),
|
||||
std::nan(""), std::nan("")};
|
||||
std::vector<std::optional<Eigen::Matrix<double, 3, 1>>> ipts{
|
||||
{std::nullopt, std::nullopt, std::nullopt, std::nullopt}};
|
||||
|
||||
for (size_t i = 0; i < viewplanes.size(); i++) {
|
||||
Eigen::Matrix<double, 3, 1> normal = viewplanes[i];
|
||||
Eigen::Matrix<double, 3, 1> 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<double, 3, 1> apv{};
|
||||
apv = intersections[i] * abv;
|
||||
Eigen::Matrix<double, 3, 1> intersectpt{};
|
||||
intersectpt = av + apv;
|
||||
ipts[i] = intersectpt;
|
||||
|
||||
for (size_t j = 1; j < viewplanes.size(); j++) {
|
||||
int oi = (i + j) % viewplanes.size();
|
||||
Eigen::Matrix<double, 3, 1> 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<Eigen::Matrix<double, 3, 1>> oipt = ipts[j];
|
||||
if (!oipt) {
|
||||
continue;
|
||||
}
|
||||
Eigen::Matrix<double, 3, 1> 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<cv::Point2f> EstPixelNoise(
|
||||
const std::vector<cv::Point2f>& points) {
|
||||
if (avgErrorPx == 0 && errorStdDevPx == 0) {
|
||||
return points;
|
||||
}
|
||||
|
||||
std::vector<cv::Point2f> 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<double, 5, 1>{
|
||||
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<double, 5, 1>{
|
||||
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<double, 5, 1>{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<double, 5, 1>{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<double, 5, 1>{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<float> gaussian{0.0, 1.0};
|
||||
std::uniform_real_distribution<float> uniform{0.0, 1.0};
|
||||
|
||||
int resWidth;
|
||||
int resHeight;
|
||||
Eigen::Matrix<double, 3, 3> camIntrinsics;
|
||||
Eigen::Matrix<double, 5, 1> 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<Eigen::Matrix<double, 3, 1>> viewplanes{};
|
||||
};
|
||||
} // namespace photon
|
||||
@@ -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 <cscore_cv.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <numeric>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <frc/apriltag/AprilTag.h>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
#include <opencv2/objdetect.hpp>
|
||||
#include <units/length.h>
|
||||
|
||||
#include "SimCameraProperties.h"
|
||||
#include "photon/estimation/RotTrlTransform3d.h"
|
||||
|
||||
namespace mathutil {
|
||||
template <typename T>
|
||||
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<int, cv::Mat> LoadAprilTagImages() {
|
||||
std::unordered_map<int, cv::Mat> retVal{};
|
||||
for (int i = 0; i < kNumTags16h5; i++) {
|
||||
cv::Mat tagImage = Get16h5TagImage(i);
|
||||
retVal[i] = tagImage;
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
static std::vector<cv::Point2f> GetImageCorners(const cv::Size& size) {
|
||||
std::vector<cv::Point2f> 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<cv::Point2f> 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<cv::Point2f> 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<cv::Point2f> Get16h5MarkerPts() {
|
||||
return Get16h5MarkerPts(1);
|
||||
}
|
||||
|
||||
static const std::unordered_map<int, cv::Mat> kTag16h5Images =
|
||||
LoadAprilTagImages();
|
||||
static const std::vector<cv::Point2f> kTag16h5MarkPts = Get16h5MarkerPts();
|
||||
|
||||
[[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video,
|
||||
const SimCameraProperties& prop) {
|
||||
video.SetResolution(prop.GetResWidth(), prop.GetResHeight());
|
||||
video.SetFPS(prop.GetFPS().to<int>());
|
||||
}
|
||||
|
||||
[[maybe_unused]] static void Warp165h5TagImage(
|
||||
int tagId, const std::vector<cv::Point2f>& dstPoints, bool antialiasing,
|
||||
cv::Mat& destination) {
|
||||
if (!kTag16h5Images.contains(tagId)) {
|
||||
return;
|
||||
}
|
||||
cv::Mat tagImage = kTag16h5Images.at(tagId);
|
||||
std::vector<cv::Point2f> tagPoints{kTag16h5MarkPts};
|
||||
std::vector<cv::Point2f> tagImageCorners{GetImageCorners(tagImage.size())};
|
||||
std::vector<cv::Point2f> dstPointMat = dstPoints;
|
||||
cv::Rect boundingRect = cv::boundingRect(dstPointMat);
|
||||
cv::Mat perspecTrf = cv::getPerspectiveTransform(tagPoints, dstPointMat);
|
||||
std::vector<cv::Point2f> 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<int>(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<int>(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<cv::Point2f> scaledDstPts{};
|
||||
if (supersampling > 1) {
|
||||
cv::multiply(dstPointMat,
|
||||
cv::Scalar{static_cast<double>(supersampling),
|
||||
static_cast<double>(supersampling)},
|
||||
scaledDstPts);
|
||||
boundingRect.x *= supersampling;
|
||||
boundingRect.y *= supersampling;
|
||||
boundingRect.width *= supersampling;
|
||||
boundingRect.height *= supersampling;
|
||||
} else {
|
||||
scaledDstPts = dstPointMat;
|
||||
}
|
||||
|
||||
cv::subtract(scaledDstPts,
|
||||
cv::Scalar{static_cast<double>(boundingRect.tl().x),
|
||||
static_cast<double>(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<float>(boundingRect.tl().x),
|
||||
static_cast<float>(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<cv::Point> 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<cv::Point2f>& 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<cv::Point2f>& dstPoints, int thickness,
|
||||
const cv::Scalar& color, bool isClosed,
|
||||
cv::Mat& destination) {
|
||||
std::vector<cv::Point> intDstPoints{dstPoints.begin(), dstPoints.end()};
|
||||
std::vector<std::vector<cv::Point>> 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<cv::Point2f>& dstPoints, cv::Mat& destination) {
|
||||
double thickness = GetScaledThickness(1, destination);
|
||||
DrawPoly(dstPoints, static_cast<int>(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<int>(thickness), cv::LINE_AA);
|
||||
}
|
||||
|
||||
static std::vector<std::vector<frc::Translation3d>> GetFieldWallLines() {
|
||||
std::vector<std::vector<frc::Translation3d>> 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>{
|
||||
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>{
|
||||
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>{
|
||||
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>{
|
||||
frc::Translation3d{fieldLength, 0_m, driveHt},
|
||||
frc::Translation3d{fieldLength, fieldWidth, driveHt}});
|
||||
|
||||
// left side wall
|
||||
list.emplace_back(std::vector<frc::Translation3d>{
|
||||
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>{
|
||||
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>{
|
||||
frc::Translation3d{0_m, 0_m, driveHt},
|
||||
frc::Translation3d{0_m, fieldWidth, driveHt}});
|
||||
|
||||
return list;
|
||||
}
|
||||
|
||||
static std::vector<std::vector<frc::Translation3d>> GetFieldFloorLines(
|
||||
int subdivisions) {
|
||||
std::vector<std::vector<frc::Translation3d>> 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>{
|
||||
frc::Translation3d{0_m, subWidth * (i + 1), 0_m},
|
||||
frc::Translation3d{fieldLength, subWidth * (i + 1), 0_m}});
|
||||
list.emplace_back(std::vector<frc::Translation3d>{
|
||||
frc::Translation3d{subLength * (i + 1), 0_m, 0_m},
|
||||
frc::Translation3d{subLength * (i + 1), fieldWidth, 0_m}});
|
||||
}
|
||||
return list;
|
||||
}
|
||||
|
||||
static std::vector<std::vector<cv::Point2f>> PolyFrom3dLines(
|
||||
const RotTrlTransform3d& camRt, const SimCameraProperties& prop,
|
||||
const std::vector<frc::Translation3d>& trls, double resolution,
|
||||
bool isClosed, cv::Mat& destination) {
|
||||
resolution = std::hypot(destination.size().height, destination.size().width) *
|
||||
resolution;
|
||||
std::vector<frc::Translation3d> pts{trls};
|
||||
if (isClosed) {
|
||||
pts.emplace_back(pts[0]);
|
||||
}
|
||||
std::vector<std::vector<cv::Point2f>> 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<double>, std::optional<double>> 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<cv::Point2f> 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<int>(pxDist / resolution);
|
||||
frc::Translation3d subDelta = baseDelta / (subdivisions + 1);
|
||||
std::vector<frc::Translation3d> subPts{};
|
||||
for (int j = 0; j < subdivisions; j++) {
|
||||
subPts.emplace_back(pta + (subDelta * (j + 1)));
|
||||
}
|
||||
if (subPts.size() > 0) {
|
||||
std::vector<cv::Point2f> 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<int>(
|
||||
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<int>(
|
||||
std::round(GetScaledThickness(wallThickness, destination))),
|
||||
wallColor, false, destination);
|
||||
}
|
||||
}
|
||||
}
|
||||
} // namespace VideoSimUtil
|
||||
} // namespace photon
|
||||
@@ -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 <string>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <frc/Timer.h>
|
||||
#include <frc/interpolation/TimeInterpolatableBuffer.h>
|
||||
#include <frc/smartdashboard/Field2d.h>
|
||||
#include <frc/smartdashboard/FieldObject2d.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
#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<PhotonCameraSim*> 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<PhotonCameraSim*> GetCameraSims() {
|
||||
std::vector<PhotonCameraSim*> 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<frc::Pose3d>{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<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim) {
|
||||
return GetRobotToCamera(cameraSim, frc::Timer::GetFPGATimestamp());
|
||||
}
|
||||
std::optional<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim,
|
||||
units::second_t time) {
|
||||
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
|
||||
frc::TimeInterpolatableBuffer<frc::Pose3d> trfBuffer =
|
||||
camTrfMap.at(cameraSim);
|
||||
std::optional<frc::Pose3d> 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<frc::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim) {
|
||||
return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp());
|
||||
}
|
||||
std::optional<frc::Pose3d> 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<VisionTargetSim> GetVisionTargets() {
|
||||
std::vector<VisionTargetSim> all{};
|
||||
for (const auto& entry : targetSets) {
|
||||
for (const auto& target : entry.second) {
|
||||
all.emplace_back(target);
|
||||
}
|
||||
}
|
||||
return all;
|
||||
}
|
||||
std::vector<VisionTargetSim> GetVisionTargets(std::string type) {
|
||||
return targetSets[type];
|
||||
}
|
||||
void AddVisionTargets(const std::vector<VisionTargetSim>& targets) {
|
||||
AddVisionTargets("targets", targets);
|
||||
}
|
||||
void AddVisionTargets(std::string type,
|
||||
const std::vector<VisionTargetSim>& targets) {
|
||||
if (!targetSets.contains(type)) {
|
||||
targetSets[type] = std::vector<VisionTargetSim>{};
|
||||
}
|
||||
for (const auto& tgt : targets) {
|
||||
targetSets[type].emplace_back(tgt);
|
||||
}
|
||||
}
|
||||
void AddAprilTags(const frc::AprilTagFieldLayout& layout) {
|
||||
std::vector<VisionTargetSim> 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<VisionTargetSim> RemoveVisionTargets(
|
||||
const std::vector<VisionTargetSim>& targets) {
|
||||
std::vector<VisionTargetSim> 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<frc::Pose2d> 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<VisionTargetSim> allTargets{};
|
||||
for (const auto& set : targetSets) {
|
||||
for (const auto& target : set.second) {
|
||||
allTargets.emplace_back(target);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<frc::Pose2d> visTgtPoses2d{};
|
||||
std::vector<frc::Pose2d> 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<double>(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<std::string, PhotonCameraSim*> camSimMap{};
|
||||
static constexpr units::second_t bufferLength{1.5_s};
|
||||
std::unordered_map<PhotonCameraSim*,
|
||||
frc::TimeInterpolatableBuffer<frc::Pose3d>>
|
||||
camTrfMap;
|
||||
frc::TimeInterpolatableBuffer<frc::Pose3d> robotPoseBuffer{bufferLength};
|
||||
std::unordered_map<std::string, std::vector<VisionTargetSim>> targetSets{};
|
||||
frc::Field2d dbgField{};
|
||||
const frc::Transform3d kEmptyTrf{};
|
||||
};
|
||||
} // namespace photon
|
||||
@@ -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 <vector>
|
||||
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
|
||||
#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<frc::Translation3d> 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
|
||||
461
photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
Normal file
461
photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
Normal file
@@ -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<units::degree_t> {};
|
||||
class VisionSystemSimTestDistanceParamsTest
|
||||
: public VisionSystemSimTest,
|
||||
public testing::WithParamInterface<
|
||||
std::tuple<units::foot_t, units::degree_t, units::foot_t>> {};
|
||||
|
||||
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<double>(),
|
||||
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<double>(),
|
||||
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<double>(),
|
||||
distParam.convert<units::meters>().to<double>(), 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<const photon::PhotonTrackedTarget> tgtList = res.GetTargets();
|
||||
ASSERT_EQ(static_cast<size_t>(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<frc::AprilTag> 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<double, 3, 3> camEigen;
|
||||
cv::cv2eigen(camera.GetCameraMatrix().value(), camEigen);
|
||||
|
||||
Eigen::Matrix<double, 5, 1> distEigen;
|
||||
cv::cv2eigen(camera.GetDistCoeffs().value(), distEigen);
|
||||
|
||||
auto camResults = camera.GetLatestResult();
|
||||
auto targetSpan = camResults.GetTargets();
|
||||
std::vector<photon::PhotonTrackedTarget> 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<double>(), 0.01);
|
||||
ASSERT_NEAR(1, pose.Y().to<double>(), 0.01);
|
||||
ASSERT_NEAR(0, pose.Z().to<double>(), 0.01);
|
||||
ASSERT_NEAR(units::degree_t{5}.convert<units::radians>().to<double>(),
|
||||
pose.Rotation().Z().to<double>(), 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<photon::PhotonTrackedTarget> 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<double>(), 0.01);
|
||||
ASSERT_NEAR(1, pose2.Y().to<double>(), 0.01);
|
||||
ASSERT_NEAR(0, pose2.Z().to<double>(), 0.01);
|
||||
ASSERT_NEAR(units::degree_t{5}.convert<units::radians>().to<double>(),
|
||||
pose2.Rotation().Z().to<double>(), 0.01);
|
||||
}
|
||||
@@ -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 <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
|
||||
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<double>(),
|
||||
camToTarg.Y().to<double>()}),
|
||||
camToTargPitch(frc::Rotation2d{camToTargDistXY.to<double>(),
|
||||
-camToTarg.Z().to<double>()}),
|
||||
camToTargAngle(frc::Rotation2d{units::math::hypot(
|
||||
camToTargYaw.Radians(), camToTargPitch.Radians())}),
|
||||
targToCam(frc::Transform3d{targetPose, cameraPose}),
|
||||
targToCamYaw(frc::Rotation2d{targToCam.X().to<double>(),
|
||||
targToCam.Y().to<double>()}),
|
||||
targToCamPitch(frc::Rotation2d{camToTargDistXY.to<double>(),
|
||||
-targToCam.Z().to<double>()}),
|
||||
targToCamAngle(frc::Rotation2d{units::math::hypot(
|
||||
targToCamYaw.Radians(), targToCamPitch.Radians())}) {}
|
||||
};
|
||||
} // namespace photon
|
||||
@@ -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 <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <opencv2/calib3d.hpp>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
#include "RotTrlTransform3d.h"
|
||||
|
||||
#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT
|
||||
#include <opencv2/core/eigen.hpp>
|
||||
#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<cv::Point2f> GetConvexHull(
|
||||
const std::vector<cv::Point2f>& points) {
|
||||
std::vector<int> outputHull{};
|
||||
cv::convexHull(points, outputHull);
|
||||
std::vector<cv::Point2f> 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<cv::Point2f>& 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<cv::Point3f> TranslationToTVec(
|
||||
const std::vector<frc::Translation3d>& translations) {
|
||||
std::vector<cv::Point3f> 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<float>(), trl.Y().to<float>(),
|
||||
trl.Z().to<float>()});
|
||||
}
|
||||
return retVal;
|
||||
}
|
||||
|
||||
static std::vector<cv::Point3f> RotationToRVec(
|
||||
const frc::Rotation3d& rotation) {
|
||||
std::vector<cv::Point3f> 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<cv::Point2f> points) {
|
||||
if (points.size() == 0) {
|
||||
return cv::Point2f{};
|
||||
}
|
||||
cv::reduce(points, points, 0, cv::REDUCE_AVG);
|
||||
return points[0];
|
||||
}
|
||||
|
||||
[[maybe_unused]] static std::vector<std::pair<float, float>> PointsToCorners(
|
||||
const std::vector<cv::Point2f>& points) {
|
||||
std::vector<std::pair<float, float>> 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<cv::Point2f> CornersToPoints(
|
||||
const std::vector<std::pair<float, float>>& corners) {
|
||||
std::vector<cv::Point2f> 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<cv::Point2f>& points) {
|
||||
return cv::boundingRect(points);
|
||||
}
|
||||
|
||||
static std::vector<cv::Point2f> ProjectPoints(
|
||||
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
|
||||
const Eigen::Matrix<double, 5, 1>& distCoeffs,
|
||||
const RotTrlTransform3d& camRt,
|
||||
const std::vector<frc::Translation3d>& objectTranslations) {
|
||||
std::vector<cv::Point3f> objectPoints = TranslationToTVec(objectTranslations);
|
||||
std::vector<cv::Point3f> rvec = RotationToRVec(camRt.GetRotation());
|
||||
std::vector<cv::Point3f> 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<cv::Point2f> imagePoints{};
|
||||
cv::projectPoints(objectPoints, rvec, tvec, cameraMat, distCoeffsMat,
|
||||
imagePoints);
|
||||
return imagePoints;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
static std::vector<T> ReorderCircular(const std::vector<T> elements,
|
||||
bool backwards, int shiftStart) {
|
||||
size_t size = elements.size();
|
||||
int dir = backwards ? -1 : 1;
|
||||
std::vector<T> 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::Vec3f>(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::Vec3f>(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<double, 3, 3>& cameraMatrix,
|
||||
const Eigen::Matrix<double, 5, 1>& distCoeffs,
|
||||
std::vector<frc::Translation3d> modelTrls,
|
||||
std::vector<cv::Point2f> imagePoints) {
|
||||
modelTrls = ReorderCircular(modelTrls, true, -1);
|
||||
imagePoints = ReorderCircular(imagePoints, true, -1);
|
||||
std::vector<cv::Point3f> objectMat = TranslationToTVec(modelTrls);
|
||||
std::vector<cv::Mat> rvecs;
|
||||
std::vector<cv::Mat> 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<frc::Transform3d> 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::Vec2f>(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<double, 3, 3>& cameraMatrix,
|
||||
const Eigen::Matrix<double, 5, 1>& distCoeffs,
|
||||
std::vector<frc::Translation3d> modelTrls,
|
||||
std::vector<cv::Point2f> imagePoints) {
|
||||
std::vector<cv::Point3f> objectMat = TranslationToTVec(modelTrls);
|
||||
std::vector<cv::Mat> rvecs{};
|
||||
std::vector<cv::Mat> 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<float>(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
|
||||
@@ -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 <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
#include <frc/geometry/Rotation3d.h>
|
||||
#include <frc/geometry/Translation3d.h>
|
||||
|
||||
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<frc::Translation3d> ApplyTrls(
|
||||
const std::vector<frc::Translation3d>& trls) const {
|
||||
std::vector<frc::Translation3d> 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<frc::Rotation3d> ApplyTrls(
|
||||
const std::vector<frc::Rotation3d>& rots) const {
|
||||
std::vector<frc::Rotation3d> 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<frc::Pose3d> ApplyPoses(
|
||||
const std::vector<frc::Pose3d>& poses) const {
|
||||
std::vector<frc::Pose3d> 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
|
||||
@@ -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 <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
#include <frc/geometry/Translation3d.h>
|
||||
|
||||
#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<frc::Translation3d>& verts)
|
||||
: isSpherical(false) {
|
||||
if (verts.size() <= 2) {
|
||||
vertices = std::vector<frc::Translation3d>();
|
||||
isPlanar = false;
|
||||
} else {
|
||||
bool cornersPlanar = true;
|
||||
for (const auto& corner : verts) {
|
||||
if (corner.X() != 0_m) {
|
||||
cornersPlanar = false;
|
||||
}
|
||||
}
|
||||
isPlanar = cornersPlanar;
|
||||
}
|
||||
vertices = verts;
|
||||
}
|
||||
|
||||
std::vector<frc::Translation3d> GetFieldVertices(
|
||||
const frc::Pose3d& targetPose) const {
|
||||
RotTrlTransform3d basisChange{targetPose.Rotation(),
|
||||
targetPose.Translation()};
|
||||
std::vector<frc::Translation3d> 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<double>(),
|
||||
-relCam.Z().to<double>()}
|
||||
.Radians(),
|
||||
frc::Rotation2d{relCam.X().to<double>(), relCam.Y().to<double>()}
|
||||
.Radians()};
|
||||
return frc::Pose3d{tgtTrl, orientToCam};
|
||||
}
|
||||
|
||||
std::vector<frc::Translation3d> GetVertices() const { return vertices; }
|
||||
bool GetIsPlanar() const { return isPlanar; }
|
||||
bool GetIsSpherical() const { return isSpherical; }
|
||||
|
||||
private:
|
||||
std::vector<frc::Translation3d> 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
|
||||
@@ -0,0 +1,121 @@
|
||||
/*
|
||||
* Copyright (C) Photon Vision.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <frc/apriltag/AprilTag.h>
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
|
||||
#include "OpenCVHelp.h"
|
||||
#include "TargetModel.h"
|
||||
#include "photon/targeting/MultiTargetPNPResult.h"
|
||||
#include "photon/targeting/PhotonTrackedTarget.h"
|
||||
|
||||
namespace photon {
|
||||
namespace VisionEstimation {
|
||||
|
||||
static std::vector<frc::AprilTag> GetVisibleLayoutTags(
|
||||
const std::vector<PhotonTrackedTarget>& visTags,
|
||||
const frc::AprilTagFieldLayout& layout) {
|
||||
std::vector<frc::AprilTag> 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<double, 3, 3>& cameraMatrix,
|
||||
const Eigen::Matrix<double, 5, 1>& distCoeffs,
|
||||
const std::vector<PhotonTrackedTarget>& visTags,
|
||||
const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) {
|
||||
if (visTags.size() == 0) {
|
||||
return PNPResult();
|
||||
}
|
||||
|
||||
std::vector<std::pair<float, float>> corners{};
|
||||
std::vector<frc::AprilTag> 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<cv::Point2f> 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<frc::Translation3d> 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
|
||||
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* Copyright (C) Photon Vision.
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <networktables/BooleanTopic.h>
|
||||
#include <networktables/DoubleArrayTopic.h>
|
||||
#include <networktables/DoubleTopic.h>
|
||||
#include <networktables/IntegerTopic.h>
|
||||
#include <networktables/NetworkTable.h>
|
||||
#include <networktables/RawTopic.h>
|
||||
|
||||
namespace photon {
|
||||
class NTTopicSet {
|
||||
public:
|
||||
std::shared_ptr<nt::NetworkTable> 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
|
||||
@@ -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;
|
||||
|
||||
|
||||
1
photonlib-cpp-examples/swervedriveposeestsim/.gitignore
vendored
Normal file
1
photonlib-cpp-examples/swervedriveposeestsim/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
vendordeps
|
||||
@@ -0,0 +1,6 @@
|
||||
{
|
||||
"enableCppIntellisense": true,
|
||||
"currentLanguage": "cpp",
|
||||
"projectYear": "2023",
|
||||
"teamNumber": 5
|
||||
}
|
||||
@@ -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.
|
||||
121
photonlib-cpp-examples/swervedriveposeestsim/build.gradle
Normal file
121
photonlib-cpp-examples/swervedriveposeestsim/build.gradle
Normal file
@@ -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)
|
||||
}
|
||||
}
|
||||
}
|
||||
241
photonlib-cpp-examples/swervedriveposeestsim/gradlew
vendored
Normal file
241
photonlib-cpp-examples/swervedriveposeestsim/gradlew
vendored
Normal file
@@ -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" "$@"
|
||||
91
photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat
vendored
Normal file
91
photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat
vendored
Normal file
@@ -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
|
||||
@@ -0,0 +1 @@
|
||||
[]
|
||||
30
photonlib-cpp-examples/swervedriveposeestsim/settings.gradle
Normal file
30
photonlib-cpp-examples/swervedriveposeestsim/settings.gradle
Normal file
@@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
98
photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json
Normal file
98
photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json
Normal file
@@ -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
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -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"
|
||||
}
|
||||
}
|
||||
}
|
||||
57
photonlib-cpp-examples/swervedriveposeestsim/simgui.json
Normal file
57
photonlib-cpp-examples/swervedriveposeestsim/simgui.json
Normal file
@@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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 <iostream>
|
||||
|
||||
#include <frc/simulation/BatterySim.h>
|
||||
#include <frc/simulation/RoboRioSim.h>
|
||||
|
||||
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<Robot>(); }
|
||||
#endif
|
||||
@@ -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 <iostream>
|
||||
|
||||
#include <frc/TimedRobot.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
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<frc::SwerveModuleState, 4>& desiredStates, bool openLoop,
|
||||
bool steerInPlace) {
|
||||
std::array<frc::SwerveModuleState, 4> desaturatedStates = desiredStates;
|
||||
frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
|
||||
static_cast<wpi::array<frc::SwerveModuleState, 4>*>(&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<double, 3> 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<frc::SwerveModuleState, 4> SwerveDrive::GetModuleStates() const {
|
||||
std::array<frc::SwerveModuleState, 4> 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<frc::SwerveModulePosition, 4> SwerveDrive::GetModulePositions()
|
||||
const {
|
||||
std::array<frc::SwerveModulePosition, 4> 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<frc::Pose2d, 4> SwerveDrive::GetModulePoses() const {
|
||||
std::array<frc::Pose2d, 4> 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<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Heading",
|
||||
pose.Rotation().Degrees().to<double>());
|
||||
frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds();
|
||||
frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Omega Degrees",
|
||||
chassisSpeeds.omega.convert<units::degrees_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VX",
|
||||
targetChassisSpeeds.vx.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Target VY",
|
||||
targetChassisSpeeds.vy.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Target Omega Degrees",
|
||||
targetChassisSpeeds.omega.convert<units::degrees_per_second>()
|
||||
.to<double>());
|
||||
|
||||
for (auto& module : swerveMods) {
|
||||
module.Log();
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDrive::SimulationPeriodic() {
|
||||
std::array<units::volt_t, 4> driveInputs;
|
||||
std::array<units::volt_t, 4> 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<units::ampere_t, 4> driveCurrents =
|
||||
swerveDriveSim.GetDriveCurrentDraw();
|
||||
for (const auto& current : driveCurrents) {
|
||||
totalCurrentDraw += current;
|
||||
}
|
||||
std::array<units::ampere_t, 4> 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; }
|
||||
@@ -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 <iostream>
|
||||
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/system/Discretization.h>
|
||||
|
||||
template <typename T>
|
||||
int sgn(T val) {
|
||||
return (T(0) < val) - (val < T(0));
|
||||
}
|
||||
|
||||
SwerveDriveSim::SwerveDriveSim(
|
||||
const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& kinematics)
|
||||
: SwerveDriveSim(
|
||||
frc::LinearSystem<2, 1, 2>{
|
||||
(Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0,
|
||||
-driveFF.kV.to<double>() / driveFF.kA.to<double>())
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0, 1.0 / driveFF.kA.to<double>()},
|
||||
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
|
||||
Eigen::Matrix<double, 2, 1>{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<double>() / steerFF.kA.to<double>())
|
||||
.finished(),
|
||||
Eigen::Matrix<double, 2, 1>{0.0, 1.0 / steerFF.kA.to<double>()},
|
||||
(Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(),
|
||||
Eigen::Matrix<double, 2, 1>{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<numModules>& 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<units::volt_t, numModules>& 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<units::volt_t, numModules>& 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<double, 2, 1> SwerveDriveSim::CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& 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<double>(), kS.to<double>());
|
||||
|
||||
nextStateVel += discB(1, 0) * ksSystemEffect;
|
||||
inputToStop = nextStateVel / -discB(1, 0);
|
||||
double signToStop = sgn(inputToStop);
|
||||
double inputSign = sgn(input.to<double>());
|
||||
double ksInputEffect = 0;
|
||||
|
||||
if (std::abs(ksSystemEffect) < kS.to<double>()) {
|
||||
double absInput = std::abs(input.to<double>());
|
||||
ksInputEffect =
|
||||
-std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
|
||||
} else if ((input.to<double>() * signToStop) > (inputToStop * signToStop)) {
|
||||
double absInput = std::abs(input.to<double>() - inputToStop);
|
||||
ksInputEffect =
|
||||
-std::clamp(kS.to<double>() * inputSign, -absInput, absInput);
|
||||
}
|
||||
|
||||
auto sF = Eigen::Matrix<double, 1, 1>{input.to<double>() + ksSystemEffect +
|
||||
ksInputEffect};
|
||||
auto Bu = discB * sF;
|
||||
auto retVal = Ax + Bu;
|
||||
return retVal;
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Update(units::second_t dt) {
|
||||
Eigen::Matrix<double, 2, 2> driveDiscA;
|
||||
Eigen::Matrix<double, 2, 1> driveDiscB;
|
||||
frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA,
|
||||
&driveDiscB);
|
||||
|
||||
Eigen::Matrix<double, 2, 2> steerDiscA;
|
||||
Eigen::Matrix<double, 2, 1> steerDiscB;
|
||||
frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA,
|
||||
&steerDiscB);
|
||||
|
||||
std::array<frc::SwerveModulePosition, 4> 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<double, 2, 1>{0, 0};
|
||||
steerStates[i] = Eigen::Matrix<double, 2, 1>{0, 0};
|
||||
}
|
||||
omega = 0_rad_per_s;
|
||||
}
|
||||
}
|
||||
|
||||
void SwerveDriveSim::Reset(const frc::Pose2d& pose,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
numModules>& moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>,
|
||||
numModules>& moduleSteerStates) {
|
||||
this->pose = pose;
|
||||
driveStates = moduleDriveStates;
|
||||
steerStates = moduleSteerStates;
|
||||
omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega;
|
||||
}
|
||||
|
||||
frc::Pose2d SwerveDriveSim::GetPose() const { return pose; }
|
||||
|
||||
std::array<frc::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetModulePositions() const {
|
||||
std::array<frc::SwerveModulePosition, numModules> 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<frc::SwerveModulePosition, numModules>
|
||||
SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev,
|
||||
units::radian_t steerStdDev) {
|
||||
std::array<frc::SwerveModulePosition, numModules> 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<frc::SwerveModuleState, numModules>
|
||||
SwerveDriveSim::GetModuleStates() {
|
||||
std::array<frc::SwerveModuleState, numModules> 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<Eigen::Matrix<double, 2, 1>, numModules>
|
||||
SwerveDriveSim::GetDriveStates() const {
|
||||
return driveStates;
|
||||
}
|
||||
|
||||
std::array<Eigen::Matrix<double, 2, 1>, 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<units::ampere_t, numModules> SwerveDriveSim::GetDriveCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> 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<double>();
|
||||
currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i],
|
||||
frc::RobotController::GetBatteryVoltage());
|
||||
}
|
||||
return currents;
|
||||
}
|
||||
|
||||
std::array<units::ampere_t, numModules> SwerveDriveSim::GetSteerCurrentDraw()
|
||||
const {
|
||||
std::array<units::ampere_t, numModules> 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;
|
||||
}
|
||||
@@ -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 <iostream>
|
||||
|
||||
#include <frc/MathUtil.h>
|
||||
#include <frc/RobotController.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
|
||||
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<double>());
|
||||
steerEncoder.SetDistancePerPulse(
|
||||
constants::Swerve::kSteerRadPerPulse.to<double>());
|
||||
steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi);
|
||||
}
|
||||
|
||||
void SwerveModule::Periodic() {
|
||||
units::volt_t steerPID = units::volt_t{
|
||||
steerPIDController.Calculate(GetAbsoluteHeading().Radians().to<double>(),
|
||||
desiredState.angle.Radians().to<double>())};
|
||||
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<double>())};
|
||||
}
|
||||
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<units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Steer Target Degrees",
|
||||
units::radian_t{steerPIDController.GetSetpoint()}
|
||||
.convert<units::degrees>()
|
||||
.to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Feet",
|
||||
state.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(
|
||||
table + "Drive Velocity Target Feet",
|
||||
desiredState.speed.convert<units::feet_per_second>().to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Drive Current",
|
||||
driveCurrentSim.to<double>());
|
||||
frc::SmartDashboard::PutNumber(table + "Steer Current",
|
||||
steerCurrentSim.to<double>());
|
||||
}
|
||||
|
||||
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<double>());
|
||||
driveEncoderSim.SetRate(driveEncoderRate.to<double>());
|
||||
driveCurrentSim = driveCurrent;
|
||||
steerEncoderSim.SetDistance(steerEncoderDist.to<double>());
|
||||
steerEncoderSim.SetRate(steerEncoderRate.to<double>());
|
||||
steerCurrentSim = steerCurrent;
|
||||
}
|
||||
@@ -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.
|
||||
@@ -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 <numbers>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/geometry/Transform3d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <units/length.h>
|
||||
|
||||
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<double, 3, 1> kSingleTagStdDevs{4, 4, 8};
|
||||
static const Eigen::Matrix<double, 3, 1> 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<units::meters> kDriveFF{
|
||||
0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq};
|
||||
|
||||
static const frc::SimpleMotorFeedforward<units::radians> 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
|
||||
@@ -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 <frc/TimedRobot.h>
|
||||
#include <frc/Timer.h>
|
||||
#include <frc/XboxController.h>
|
||||
#include <frc/filter/SlewRateLimiter.h>
|
||||
|
||||
#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<units::scalar> forwardLimiter{1.0 / 0.6_s};
|
||||
frc::SlewRateLimiter<units::scalar> strafeLimiter{1.0 / 0.6_s};
|
||||
frc::SlewRateLimiter<units::scalar> turnLimiter{1.0 / 0.33_s};
|
||||
frc::Timer autoTimer{};
|
||||
double kDriveSpeed{0.6};
|
||||
};
|
||||
@@ -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 <photon/PhotonCamera.h>
|
||||
#include <photon/PhotonPoseEstimator.h>
|
||||
#include <photon/estimation/VisionEstimation.h>
|
||||
#include <photon/simulation/VisionSystemSim.h>
|
||||
#include <photon/simulation/VisionTargetSim.h>
|
||||
#include <photon/targeting/PhotonPipelineResult.h>
|
||||
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
|
||||
#include <frc/apriltag/AprilTagFieldLayout.h>
|
||||
#include <frc/apriltag/AprilTagFields.h>
|
||||
|
||||
#include "Constants.h"
|
||||
|
||||
class Vision {
|
||||
public:
|
||||
Vision() {
|
||||
photonEstimator.SetMultiTagFallbackStrategy(
|
||||
photon::PoseStrategy::LOWEST_AMBIGUITY);
|
||||
|
||||
if (frc::RobotBase::IsSimulation()) {
|
||||
visionSim = std::make_unique<photon::VisionSystemSim>("main");
|
||||
|
||||
visionSim->AddAprilTags(constants::Vision::kTagLayout);
|
||||
|
||||
cameraProp = std::make_unique<photon::SimCameraProperties>();
|
||||
|
||||
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<photon::PhotonCameraSim>(camera.get(),
|
||||
*cameraProp.get());
|
||||
|
||||
visionSim->AddCamera(cameraSim.get(), robotToCam);
|
||||
cameraSim->EnableDrawWireframe(true);
|
||||
}
|
||||
}
|
||||
|
||||
photon::PhotonPipelineResult GetLatestResult() {
|
||||
return camera->GetLatestResult();
|
||||
}
|
||||
|
||||
std::optional<photon::EstimatedRobotPose> 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<double, 3, 1> GetEstimationStdDevs(frc::Pose2d estimatedPose) {
|
||||
Eigen::Matrix<double, 3, 1> 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<double>::max(),
|
||||
std::numeric_limits<double>::max(),
|
||||
std::numeric_limits<double>::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<photon::PhotonCamera> camera{photonEstimator.GetCamera()};
|
||||
std::unique_ptr<photon::VisionSystemSim> visionSim;
|
||||
std::unique_ptr<photon::SimCameraProperties> cameraProp;
|
||||
std::shared_ptr<photon::PhotonCameraSim> cameraSim;
|
||||
units::second_t lastEstTimestamp{0_s};
|
||||
};
|
||||
@@ -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 <frc/ADXRS450_Gyro.h>
|
||||
#include <frc/SPI.h>
|
||||
#include <frc/estimator/SwerveDrivePoseEstimator.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/simulation/ADXRS450_GyroSim.h>
|
||||
|
||||
#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<frc::SwerveModuleState, 4>& 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<frc::SwerveModuleState, 4> GetModuleStates() const;
|
||||
std::array<frc::SwerveModulePosition, 4> GetModulePositions() const;
|
||||
std::array<frc::Pose2d, 4> GetModulePoses() const;
|
||||
void Log();
|
||||
void SimulationPeriodic();
|
||||
frc::Pose2d GetSimPose() const;
|
||||
units::ampere_t GetCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::array<SwerveModule, 4> 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};
|
||||
};
|
||||
@@ -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 <random>
|
||||
|
||||
#include <frc/controller/SimpleMotorFeedforward.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/system/LinearSystem.h>
|
||||
#include <frc/system/plant/DCMotor.h>
|
||||
#include <units/voltage.h>
|
||||
|
||||
static constexpr int numModules{4};
|
||||
|
||||
class SwerveDriveSim {
|
||||
public:
|
||||
SwerveDriveSim(const frc::SimpleMotorFeedforward<units::meters>& driveFF,
|
||||
const frc::DCMotor& driveMotor, double driveGearing,
|
||||
units::meter_t driveWheelRadius,
|
||||
const frc::SimpleMotorFeedforward<units::radians>& steerFF,
|
||||
const frc::DCMotor& steerMotor, double steerGearing,
|
||||
const frc::SwerveDriveKinematics<numModules>& 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<numModules>& kinematics);
|
||||
void SetDriveInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
void SetSteerInputs(const std::array<units::volt_t, numModules>& inputs);
|
||||
static Eigen::Matrix<double, 2, 1> CalculateX(
|
||||
const Eigen::Matrix<double, 2, 2>& discA,
|
||||
const Eigen::Matrix<double, 2, 1>& discB,
|
||||
const Eigen::Matrix<double, 2, 1>& 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<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleDriveStates,
|
||||
const std::array<Eigen::Matrix<double, 2, 1>, numModules>&
|
||||
moduleSteerStates);
|
||||
frc::Pose2d GetPose() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetModulePositions() const;
|
||||
std::array<frc::SwerveModulePosition, numModules> GetNoisyModulePositions(
|
||||
units::meter_t driveStdDev, units::radian_t steerStdDev);
|
||||
std::array<frc::SwerveModuleState, numModules> GetModuleStates();
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> GetDriveStates() const;
|
||||
std::array<Eigen::Matrix<double, 2, 1>, 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<units::ampere_t, numModules> GetDriveCurrentDraw() const;
|
||||
std::array<units::ampere_t, numModules> GetSteerCurrentDraw() const;
|
||||
units::ampere_t GetTotalCurrentDraw() const;
|
||||
|
||||
private:
|
||||
std::random_device rd{};
|
||||
std::mt19937 generator{rd()};
|
||||
std::normal_distribution<double> 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<numModules> kinematics;
|
||||
std::array<units::volt_t, numModules> driveInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> driveStates{};
|
||||
std::array<units::volt_t, numModules> steerInputs{};
|
||||
std::array<Eigen::Matrix<double, 2, 1>, numModules> steerStates{};
|
||||
frc::Pose2d pose{frc::Pose2d{}};
|
||||
units::radians_per_second_t omega{0};
|
||||
};
|
||||
@@ -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 <frc/Encoder.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/kinematics/SwerveModulePosition.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/motorcontrol/PWMSparkMax.h>
|
||||
#include <frc/simulation/EncoderSim.h>
|
||||
#include <units/current.h>
|
||||
|
||||
#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};
|
||||
};
|
||||
@@ -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 <hal/HAL.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
BIN
photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png
Normal file
BIN
photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 2.9 KiB |
BIN
photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png
Normal file
BIN
photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 4.6 KiB |
BIN
photonlib-cpp-examples/swervedriveposeestsim/tag-green.png
Normal file
BIN
photonlib-cpp-examples/swervedriveposeestsim/tag-green.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 4.6 KiB |
@@ -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")
|
||||
|
||||
Reference in New Issue
Block a user