Skip to content

Commit

Permalink
Create GetAllUnreadResult
Browse files Browse the repository at this point in the history
  • Loading branch information
@mcm001
mcm001 committed May 5, 2024
1 parent a8c905c commit 6c953ab
Show file tree
Hide file tree
Showing 6 changed files with 65 additions and 180 deletions.
32 changes: 17 additions & 15 deletions photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
rootTable->GetRawTopic("rawBytes")
.Subscribe(
"rawBytes", {},
{.pollStorage = 1, .periodic = 0.01, .sendAll = true})),
{.pollStorage = 20, .periodic = 0.01, .sendAll = true})),
inputSaveImgEntry(
rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()),
inputSaveImgSubscriber(
Expand Down Expand Up @@ -102,7 +102,7 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
PhotonCamera::PhotonCamera(const std::string_view cameraName)
: PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}

PhotonPipelineResult PhotonCamera::GetLatestResult() {
std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResult() {
if (test) {
return testResult;
}
Expand All @@ -114,25 +114,27 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
packet.Clear();

// Create the new result;
PhotonPipelineResult result;
std::vector<PhotonPipelineResult> ret;

// Fill the packet with latest data and populate result.
const auto changes = rawBytesEntry.ReadQueue();
if (!changes.size()) {
return result;
}
const nt::Timestamped<std::vector<uint8_t>> value = changes[0];
if (!value.value.size() || value.time == 0) {
return result;
}
ret.reserve(changes.size());

photon::Packet packet{value.value};
for (const nt::Timestamped<std::vector<uint8_t>>& value : changes) {
if (!value.value.size() || value.time == 0) {
continue;
}

packet >> result;
// Fill the packet with latest data and populate result.
photon::Packet packet{value.value};

result.SetTimestamp(units::microsecond_t(value.time) - result.GetLatency());
PhotonPipelineResult result;
packet >> result;
result.SetTimestamp(units::microsecond_t(value.time) - result.GetLatency());

ret.emplace_back(std::move(result));
}

return result;
return ret;
}

void PhotonCamera::SetDriverMode(bool driverMode) {
Expand Down
12 changes: 10 additions & 2 deletions photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,8 +112,16 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
"");
return std::nullopt;
}
auto result = camera->GetLatestResult();
return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());

auto results = camera->GetAllUnreadResult();

std::optional<EstimatedRobotPose> ret;

for (const auto& result : results) {
ret = Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());
}

return ret;
}

std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
Expand Down
10 changes: 5 additions & 5 deletions photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
}

bool PhotonCameraSim::CanSeeTargetPose(const frc::Pose3d& camPose,
const VisionTargetSim& target) {
const VisionTargetSim& target) {
CameraTargetRelation rel{camPose, target.GetPose()};
return ((units::math::abs(rel.camToTargYaw.Degrees()) <
prop.GetHorizFOV().Degrees() / 2.0) &&
Expand Down Expand Up @@ -94,9 +94,9 @@ std::optional<uint64_t> PhotonCameraSim::ConsumeNextEntryTime() {
return std::nullopt;
}
}
PhotonPipelineResult PhotonCameraSim::Process(units::second_t latency,
const frc::Pose3d& cameraPose,
std::vector<VisionTargetSim> targets) {
PhotonPipelineResult PhotonCameraSim::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 =
Expand Down Expand Up @@ -332,7 +332,7 @@ void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {
SubmitProcessedFrame(result, wpi::Now());
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
uint64_t recieveTimestamp) {
uint64_t recieveTimestamp) {
ts.latencyMillisEntry.Set(
result.GetLatency().convert<units::milliseconds>().to<double>(),
recieveTimestamp);
Expand Down
22 changes: 17 additions & 5 deletions photon-lib/src/main/native/include/photon/PhotonCamera.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@

#include <memory>
#include <string>
#include <vector>

#include <networktables/BooleanTopic.h>
#include <networktables/DoubleArrayTopic.h>
Expand Down Expand Up @@ -74,13 +75,24 @@ class PhotonCamera {

PhotonCamera(PhotonCamera&&) = default;

virtual ~PhotonCamera() = default;
~PhotonCamera() = default;

/**
* Returns the latest pipeline result.
* @return The latest pipeline result.
* The list of pipeline results sent by PhotonVision since the last call to
* GetAllUnreadResult(). Calling this function clears the internal FIFO queue,
* and multiple calls to GetAllUnreadResult() will return different
* (potentially empty) result arrays. Be careful to call this exactly ONCE per
* loop of your robot code! FIFO depth is limited to 20 changes, so make sure
* to call this frequently enough to avoid old results being discarded, too!
*/
virtual PhotonPipelineResult GetLatestResult();
std::vector<PhotonPipelineResult> GetAllUnreadResult();

[[deprecated("Replace with GetAllUnreadResult")]]
inline const PhotonPipelineResult GetLatestResult() {
const auto ret = GetAllUnreadResult();
if (!ret.size()) return PhotonPipelineResult();
return ret.back();
}

/**
* Toggles driver mode.
Expand Down Expand Up @@ -159,7 +171,7 @@ class PhotonCamera {

// For use in tests
bool test = false;
PhotonPipelineResult testResult;
std::vector<PhotonPipelineResult> testResult;

protected:
std::shared_ptr<nt::NetworkTable> mainTable;
Expand Down
137 changes: 0 additions & 137 deletions photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h
View file

This file was deleted.

32 changes: 16 additions & 16 deletions photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,8 +83,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
0.4, corners, detectedCorners}};

cameraOne.test = true;
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(11));
cameraOne.testResult = {{2_ms, targets}};
cameraOne.testResult[0].SetTimestamp(units::second_t(11));

photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
std::move(cameraOne), {});
Expand Down Expand Up @@ -138,8 +138,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
0.4, corners, detectedCorners}};

cameraOne.test = true;
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(17_s);
cameraOne.testResult = {{2_ms, targets}};
cameraOne.testResult[0].SetTimestamp(17_s);

photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne),
Expand Down Expand Up @@ -181,8 +181,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
0.4, corners, detectedCorners}};

cameraOne.test = true;
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(17));
cameraOne.testResult = {{2_ms, targets}};
cameraOne.testResult[0].SetTimestamp(units::second_t(17));

photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, std::move(cameraOne), {});
Expand Down Expand Up @@ -225,8 +225,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
0.4, corners, detectedCorners}};

cameraOne.test = true;
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(17));
cameraOne.testResult = {{2_ms, targets}};
cameraOne.testResult[0].SetTimestamp(units::second_t(17));

photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
std::move(cameraOne), {});
Expand Down Expand Up @@ -259,8 +259,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};

estimator.GetCamera()->testResult = {2_ms, targetsThree};
estimator.GetCamera()->testResult.SetTimestamp(units::second_t(21));
estimator.GetCamera()->testResult = {{2_ms, targetsThree}};
estimator.GetCamera()->testResult[0].SetTimestamp(units::second_t(21));

estimatedPose = estimator.Update();
ASSERT_TRUE(estimatedPose);
Expand Down Expand Up @@ -300,8 +300,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
0.4, corners, detectedCorners}};

cameraOne.test = true;
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(15));
cameraOne.testResult = {{2_ms, targets}};
cameraOne.testResult[0].SetTimestamp(units::second_t(15));

photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
std::move(cameraOne), {});
Expand Down Expand Up @@ -347,14 +347,14 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
std::move(cameraOne), {});

// empty input, expect empty out
estimator.GetCamera()->testResult = {2_ms, {}};
estimator.GetCamera()->testResult.SetTimestamp(units::second_t(1));
estimator.GetCamera()->testResult = {{2_ms, {}}};
estimator.GetCamera()->testResult[0].SetTimestamp(units::second_t(1));
auto estimatedPose = estimator.Update();
EXPECT_FALSE(estimatedPose);

// Set result, and update -- expect present and timestamp to be 15
estimator.GetCamera()->testResult = {3_ms, targets};
estimator.GetCamera()->testResult.SetTimestamp(units::second_t(15));
estimator.GetCamera()->testResult = {{3_ms, targets}};
estimator.GetCamera()->testResult[0].SetTimestamp(units::second_t(15));
estimatedPose = estimator.Update();
EXPECT_TRUE(estimatedPose);
EXPECT_NEAR(15, estimatedPose.value().timestamp.to<double>(), 1e-6);
Expand Down

0 comments on commit 6c953ab

Please sign in to comment.