Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use ReadQueue for PhotonCamera timestamps (#1316)

Browse Source 
This removes the extra GetLastChange call to keep everything properly
atomic.

Closes #1303
This commit is contained in:
Matt
2024-08-04 14:23:46 -04:00
committed by GitHub
parent 37e9d40762
commit 67463a020a
29 changed files with 1057 additions and 1614 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h
View File

@@ -33,15 +33,21 @@
#include <frc/apriltag/AprilTagFields.h>
class PhotonCameraWrapper {
private:
photon::PhotonCamera camera{"WPI2023"};
public:
photon::PhotonPoseEstimator m_poseEstimator{
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
photon::MULTI_TAG_PNP_ON_RIO, std::move(photon::PhotonCamera{"WPI2023"}),
frc::Transform3d{}};
photon::MULTI_TAG_PNP_ON_RIO, frc::Transform3d{}};
inline std::optional<photon::EstimatedRobotPose> Update(
frc::Pose2d estimatedPose) {
m_poseEstimator.SetReferencePose(frc::Pose3d(estimatedPose));
return m_poseEstimator.Update();
std::optional<photon::EstimatedRobotPose> ret = std::nullopt;
for (const auto& change : camera.GetAllUnreadResults())
ret = m_poseEstimator.Update(change);
return ret;
}
};

46
photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h
View File

@@ -58,37 +58,37 @@ class Vision {
cameraProp->SetAvgLatency(50_ms);
cameraProp->SetLatencyStdDev(15_ms);
cameraSim = std::make_shared<photon::PhotonCameraSim>(camera.get(),
*cameraProp.get());
cameraSim =
std::make_shared<photon::PhotonCameraSim>(&camera, *cameraProp.get());
visionSim->AddCamera(cameraSim.get(), constants::Vision::kRobotToCam);
cameraSim->EnableDrawWireframe(true);
}
}
photon::PhotonPipelineResult GetLatestResult() {
return camera->GetLatestResult();
}
photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; }
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) {
std::optional<photon::EstimatedRobotPose> visionEst;
// Run each new pipeline result through our pose estimator
for (const auto& result : camera.GetAllUnreadResults()) {
// cache result and update pose estimator
auto visionEst = photonEstimator.Update(result);
m_latestResult = result;
// In sim only, add our vision estimate to the sim debug field
if (frc::RobotBase::IsSimulation()) {
if (visionEst.has_value()) {
GetSimDebugField()
.GetObject("VisionEstimation")
->SetPose(visionEst.value().estimatedPose.ToPose2d());
} else {
GetSimDebugField().GetObject("VisionEstimation")->SetPoses({});
}
}
}
if (newResult) {
lastEstTimestamp = latestTimestamp;
}
return visionEst;
}
@@ -141,10 +141,12 @@ class Vision {
photon::PhotonPoseEstimator photonEstimator{
constants::Vision::kTagLayout,
photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR,
photon::PhotonCamera{"photonvision"}, constants::Vision::kRobotToCam};
std::shared_ptr<photon::PhotonCamera> camera{photonEstimator.GetCamera()};
constants::Vision::kRobotToCam};
photon::PhotonCamera camera{"photonvision"};
std::unique_ptr<photon::VisionSystemSim> visionSim;
std::unique_ptr<photon::SimCameraProperties> cameraProp;
std::shared_ptr<photon::PhotonCameraSim> cameraSim;
units::second_t lastEstTimestamp{0_s};
// The most recent result, cached for calculating std devs
photon::PhotonPipelineResult m_latestResult;
};