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

Auto-generate packet dataclasses with Jinja (#1374)

Browse Source
This commit is contained in:
Matt
2024-08-31 13:44:19 -04:00
committed by GitHub
parent c19d54c633
commit 169595e56e
140 changed files with 4445 additions and 2097 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
View File

@@ -35,6 +35,7 @@
#include <frc/Timer.h>
#include <opencv2/core.hpp>
#include <opencv2/core/mat.hpp>
#include <wpi/json.h>
#include "PhotonVersion.h"
#include "photon/dataflow/structures/Packet.h"
@@ -121,20 +122,18 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
// Prints warning if not connected
VerifyVersion();
// Create the new result;
PhotonPipelineResult result;
// Fill the packet with latest data and populate result.
units::microsecond_t now =
units::microsecond_t(frc::RobotController::GetFPGATime());
const auto value = rawBytesEntry.Get();
if (!value.size()) return result;
if (!value.size()) return PhotonPipelineResult{};
photon::Packet packet{value};
packet >> result;
// Create the new result;
PhotonPipelineResult result = packet.Unpack<PhotonPipelineResult>();
result.SetRecieveTimestamp(now);
result.SetReceiveTimestamp(now);
return result;
}
@@ -149,8 +148,9 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
const auto changes = rawBytesEntry.ReadQueue();
// Create the new result list -- these will be updated in-place
std::vector<PhotonPipelineResult> ret(changes.size());
// Create the new result list
std::vector<PhotonPipelineResult> ret;
ret.reserve(changes.size());
for (size_t i = 0; i < changes.size(); i++) {
const nt::Timestamped<std::vector<uint8_t>>& value = changes[i];
@@ -161,13 +161,14 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
// Fill the packet with latest data and populate result.
photon::Packet packet{value.value};
auto result = packet.Unpack<PhotonPipelineResult>();
PhotonPipelineResult& result = ret[i];
packet >> result;
// TODO: NT4 timestamps are still not to be trusted. But it's the best we
// can do until we can make time sync more reliable.
result.SetRecieveTimestamp(units::microsecond_t(value.time) -
result.SetReceiveTimestamp(units::microsecond_t(value.time) -
result.GetLatency());
ret.push_back(result);
}
return ret;
@@ -209,9 +210,11 @@ std::optional<PhotonCamera::CameraMatrix> PhotonCamera::GetCameraMatrix() {
auto camCoeffs = cameraIntrinsicsSubscriber.Get();
if (camCoeffs.size() == 9) {
PhotonCamera::CameraMatrix retVal =
Eigen::Map<const PhotonCamera::CameraMatrix>(camCoeffs.data());
Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
camCoeffs.data());
return retVal;
}
return std::nullopt;
}
@@ -230,22 +233,6 @@ std::optional<PhotonCamera::DistortionMatrix> PhotonCamera::GetDistCoeffs() {
return std::nullopt;
}
static bool VersionMatches(std::string them_str) {
std::smatch match;
std::regex versionPattern{"v[0-9]+.[0-9]+.[0-9]+"};
std::string us_str = PhotonVersion::versionString;
// Check that both versions are in the right format
if (std::regex_search(us_str, match, versionPattern) &&
std::regex_search(them_str, match, versionPattern)) {
// If they are, check string equality
return (us_str == them_str);
} else {
return false;
}
}
void PhotonCamera::VerifyVersion() {
if (!PhotonCamera::VERSION_CHECK_ENABLED) {
return;
@@ -282,13 +269,20 @@ void PhotonCamera::VerifyVersion() {
"Found the following PhotonVision cameras on NetworkTables:{}",
cameraNameOutString);
}
} else if (!VersionMatches(versionString)) {
FRC_ReportError(frc::warn::Warning, bfw);
std::string error_str = fmt::format(
"Photonlib version {} does not match coprocessor version {}!",
PhotonVersion::versionString, versionString);
FRC_ReportError(frc::err::Error, "{}", error_str);
throw std::runtime_error(error_str);
} else {
std::string local_uuid{SerdeType<PhotonPipelineResult>::GetSchemaHash()};
std::string remote_uuid =
rawBytesEntry.GetTopic().GetProperty("message_uuid");
if (local_uuid != remote_uuid) {
FRC_ReportError(frc::warn::Warning, bfw);
std::string error_str = fmt::format(
"Photonlib version {} (message definition version {}) does not match "
"coprocessor version {} (message definition version {})!",
PhotonVersion::versionString, local_uuid, versionString, remote_uuid);
FRC_ReportError(frc::err::Error, "{}", error_str);
throw std::runtime_error(error_str);
}
}
}

17
photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -100,6 +100,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
FRC_ReportError(frc::warn::Warning,
"Result timestamp was reported in the past!");
return std::nullopt;
}
@@ -164,7 +166,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
}
if (ret) {
lastPose = ret.value().estimatedPose;
lastPose = ret->estimatedPose;
}
return ret;
}
@@ -197,8 +199,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
}
return EstimatedRobotPose{
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
fiducialPose->TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
}
@@ -220,7 +221,7 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
target.GetFiducialId());
continue;
}
frc::Pose3d const targetPose = fiducialPose.value();
frc::Pose3d const targetPose = *fiducialPose;
units::meter_t const alternativeDifference = units::math::abs(
m_robotToCamera.Z() -
@@ -349,8 +350,8 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result) {
if (result.MultiTagResult().result.isPresent) {
const auto field2camera = result.MultiTagResult().result.best;
if (result.MultiTagResult()) {
const auto field2camera = result.MultiTagResult()->estimatedPose.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
@@ -398,8 +399,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
tagCorners.has_value()) {
auto const targetCorners = target.GetDetectedCorners();
for (size_t cornerIdx = 0; cornerIdx < 4; ++cornerIdx) {
imagePoints.emplace_back(targetCorners[cornerIdx].first,
targetCorners[cornerIdx].second);
imagePoints.emplace_back(targetCorners[cornerIdx].x,
targetCorners[cornerIdx].y);
objectPoints.emplace_back((*tagCorners)[cornerIdx]);
}
}

100
photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp
View File

@@ -201,33 +201,34 @@ PhotonPipelineResult PhotonCameraSim::Process(
continue;
}
PNPResult pnpSim{};
std::optional<photon::PnpResult> pnpSim = std::nullopt;
if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) {
pnpSim = OpenCVHelp::SolvePNP_Square(
pnpSim = OpenCVHelp::SolvePNP_SQPNP(
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;
std::vector<TargetCorner> smallVec;
for (const auto& corner : tempCorners) {
smallVec.emplace_back(std::make_pair(static_cast<double>(corner.first),
static_cast<double>(corner.second)));
smallVec.emplace_back(static_cast<double>(corner.first),
static_cast<double>(corner.second));
}
std::vector<std::pair<float, float>> cornersFloat =
OpenCVHelp::PointsToCorners(noisyTargetCorners);
auto cornersFloat = OpenCVHelp::PointsToTargetCorners(noisyTargetCorners);
std::vector<std::pair<double, double>> cornersDouble{cornersFloat.begin(),
cornersFloat.end()};
std::vector<TargetCorner> cornersDouble{cornersFloat.begin(),
cornersFloat.end()};
detectableTgts.emplace_back(
-centerRot.Z().convert<units::degrees>().to<double>(),
-centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
centerRot.X().convert<units::degrees>().to<double>(), tgt.fiducialId,
tgt.objDetClassId, tgt.objDetConf, pnpSim.best, pnpSim.alt,
pnpSim.ambiguity, smallVec, cornersDouble);
tgt.objDetClassId, tgt.objDetConf,
pnpSim ? pnpSim->best : frc::Transform3d{},
pnpSim ? pnpSim->alt : frc::Transform3d{},
pnpSim ? pnpSim->ambiguity : -1, smallVec, cornersDouble);
}
if (videoSimRawEnabled) {
@@ -275,36 +276,27 @@ PhotonPipelineResult PhotonCameraSim::Process(
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),
OpenCVHelp::CornersToPoints(detectedCornersDouble),
videoSimFrameProcessed);
} else {
cv::rectangle(videoSimFrameProcessed,
OpenCVHelp::GetBoundingRect(
OpenCVHelp::CornersToPoints(detectedCornerFloat)),
OpenCVHelp::CornersToPoints(detectedCornersDouble)),
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();
auto 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),
OpenCVHelp::CornersToPoints(smallVec),
static_cast<int>(
VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed);
@@ -316,75 +308,81 @@ PhotonPipelineResult PhotonCameraSim::Process(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}
MultiTargetPNPResult multiTagResults{};
std::optional<MultiTargetPNPResult> multiTagResults = std::nullopt;
std::vector<frc::AprilTag> visibleLayoutTags =
VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
wpi::SmallVector<int16_t, 32> usedIds{};
std::vector<int16_t> usedIds{};
usedIds.resize(visibleLayoutTags.size());
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(
auto pnpResult = VisionEstimation::EstimateCamPosePNP(
prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout,
kAprilTag36h11);
multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
if (pnpResult) {
multiTagResults = MultiTargetPNPResult{*pnpResult, usedIds};
}
}
heartbeatCounter++;
return PhotonPipelineResult{heartbeatCounter, 0_s, latency, detectableTgts,
multiTagResults};
return PhotonPipelineResult{
PhotonPipelineMetadata{heartbeatCounter, 0,
units::microsecond_t{latency}.to<int64_t>()},
detectableTgts, multiTagResults};
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {
SubmitProcessedFrame(result, wpi::Now());
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
uint64_t recieveTimestamp) {
uint64_t ReceiveTimestamp) {
ts.latencyMillisEntry.Set(
result.GetLatency().convert<units::milliseconds>().to<double>(),
recieveTimestamp);
ReceiveTimestamp);
Packet newPacket{};
newPacket << result;
newPacket.Pack(result);
ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp);
ts.rawBytesEntry.Set(newPacket.GetData(), ReceiveTimestamp);
bool hasTargets = result.HasTargets();
ts.hasTargetEntry.Set(hasTargets, recieveTimestamp);
ts.hasTargetEntry.Set(hasTargets, ReceiveTimestamp);
if (!hasTargets) {
ts.targetPitchEntry.Set(0.0, recieveTimestamp);
ts.targetYawEntry.Set(0.0, recieveTimestamp);
ts.targetAreaEntry.Set(0.0, recieveTimestamp);
ts.targetPitchEntry.Set(0.0, ReceiveTimestamp);
ts.targetYawEntry.Set(0.0, ReceiveTimestamp);
ts.targetAreaEntry.Set(0.0, ReceiveTimestamp);
std::array<double, 3> poseData{0.0, 0.0, 0.0};
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
ts.targetSkewEntry.Set(0.0, recieveTimestamp);
ts.targetPoseEntry.Set(poseData, ReceiveTimestamp);
ts.targetSkewEntry.Set(0.0, ReceiveTimestamp);
} 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);
ts.targetPitchEntry.Set(bestTarget.GetPitch(), ReceiveTimestamp);
ts.targetYawEntry.Set(bestTarget.GetYaw(), ReceiveTimestamp);
ts.targetAreaEntry.Set(bestTarget.GetArea(), ReceiveTimestamp);
ts.targetSkewEntry.Set(bestTarget.GetSkew(), ReceiveTimestamp);
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);
ts.targetPoseEntry.Set(poseData, ReceiveTimestamp);
}
auto intrinsics = prop.GetIntrinsics();
std::vector<double> intrinsicsView{intrinsics.data(),
intrinsics.data() + intrinsics.size()};
ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp);
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> intrinsics =
prop.GetIntrinsics();
std::span<double> intrinsicsView{intrinsics.data(),
intrinsics.data() + intrinsics.size()};
ts.cameraIntrinsicsPublisher.Set(intrinsicsView, ReceiveTimestamp);
auto distortion = prop.GetDistCoeffs();
std::vector<double> distortionView{distortion.data(),
distortion.data() + distortion.size()};
ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp);
ts.cameraDistortionPublisher.Set(distortionView, ReceiveTimestamp);
ts.heartbeatPublisher.Set(heartbeatCounter, recieveTimestamp);
ts.heartbeatPublisher.Set(heartbeatCounter, ReceiveTimestamp);
ts.subTable->GetInstance().Flush();
}