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);
}
}
}