Skip to content

Commit

Permalink
update photon
Browse files Browse the repository at this point in the history
  • Loading branch information
r4stered committed Sep 4, 2024
1 parent 3cae4b3 commit fdc9431
Show file tree
Hide file tree
Showing 44 changed files with 1,343 additions and 601 deletions.
6 changes: 5 additions & 1 deletion grab_photon.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,15 @@
"photon-lib/src/main/native/cpp",
"photon-lib/src/main/native/include",
"photon-targeting/src/main/native/cpp",
"photon-targeting/src/main/native/include"
"photon-targeting/src/main/native/include",
"photon-targeting/src/generated/main/native/cpp",
"photon-targeting/src/generated/main/native/include"
]

# Destination directories (change these to your project's subdirectories)
destinations = [
"src/main/cpp",
"src/main/include",
"src/main/cpp",
"src/main/include",
"src/main/cpp",
Expand Down
12 changes: 12 additions & 0 deletions src/main/cpp/PhotonVersion.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
#include <string.h>
#include <regex>

static const char* dev_ = "dev";

namespace photon {
namespace PhotonVersion {
const char* versionString = "UNKNOWN";
const char* buildDate = "UNKNOWN";
const bool isRelease = strncmp(dev_, versionString, strlen(dev_)) != 0;
}
}
137 changes: 65 additions & 72 deletions src/main/cpp/photon/PhotonCamera.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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;
}
Expand All @@ -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];
Expand All @@ -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;
Expand Down Expand Up @@ -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;
}

Expand All @@ -230,67 +233,57 @@ std::optional<PhotonCamera::DistortionMatrix> PhotonCamera::GetDistCoeffs() {
return std::nullopt;
}

static bool VersionMatches(std::string them_str) {
return true;
// 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;
// }

// if ((frc::Timer::GetFPGATimestamp() - lastVersionCheckTime) <
// VERSION_CHECK_INTERVAL)
// return;
// this->lastVersionCheckTime = frc::Timer::GetFPGATimestamp();

// const std::string& versionString = versionEntry.Get("");
// if (versionString.empty()) {
// std::string path_ = path;
// std::vector<std::string> cameraNames =
// rootTable->GetInstance().GetTable("photonvision")->GetSubTables();
// if (cameraNames.empty()) {
// FRC_ReportError(frc::warn::Warning,
// "Could not find any PhotonVision coprocessors on "
// "NetworkTables. Double check that PhotonVision is "
// "running, and that your camera is connected!");
// } else {
// FRC_ReportError(
// frc::warn::Warning,
// "PhotonVision coprocessor at path {} not found on NetworkTables. "
// "Double check that your camera names match!",
// path_);

// std::string cameraNameOutString;
// for (unsigned int i = 0; i < cameraNames.size(); i++) {
// cameraNameOutString += "\n" + cameraNames[i];
// }
// FRC_ReportError(
// frc::warn::Warning,
// "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);
// }
if (!PhotonCamera::VERSION_CHECK_ENABLED) {
return;
}

if ((frc::Timer::GetFPGATimestamp() - lastVersionCheckTime) <
VERSION_CHECK_INTERVAL)
return;
this->lastVersionCheckTime = frc::Timer::GetFPGATimestamp();

const std::string& versionString = versionEntry.Get("");
if (versionString.empty()) {
std::string path_ = path;
std::vector<std::string> cameraNames =
rootTable->GetInstance().GetTable("photonvision")->GetSubTables();
if (cameraNames.empty()) {
FRC_ReportError(frc::warn::Warning,
"Could not find any PhotonVision coprocessors on "
"NetworkTables. Double check that PhotonVision is "
"running, and that your camera is connected!");
} else {
FRC_ReportError(
frc::warn::Warning,
"PhotonVision coprocessor at path {} not found on NetworkTables. "
"Double check that your camera names match!",
path_);

std::string cameraNameOutString;
for (unsigned int i = 0; i < cameraNames.size(); i++) {
cameraNameOutString += "\n" + cameraNames[i];
}
FRC_ReportError(
frc::warn::Warning,
"Found the following PhotonVision cameras on NetworkTables:{}",
cameraNameOutString);
}
} 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);
}
}
}

std::vector<std::string> PhotonCamera::tablesThatLookLikePhotonCameras() {
Expand Down
17 changes: 9 additions & 8 deletions src/main/cpp/photon/PhotonPoseEstimator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -164,7 +166,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
}

if (ret) {
lastPose = ret.value().estimatedPose;
lastPose = ret->estimatedPose;
}
return ret;
}
Expand Down Expand Up @@ -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};
}
Expand All @@ -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() -
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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]);
}
}
Expand Down
39 changes: 39 additions & 0 deletions src/main/cpp/photon/serde/MultiTargetPNPResultSerde.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
/*
* 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/>.
*/

// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY

#include "photon/serde/MultiTargetPNPResultSerde.h"

namespace photon {

using StructType = SerdeType<MultiTargetPNPResult>;

void StructType::Pack(Packet& packet, const MultiTargetPNPResult& value) {
packet.Pack<photon::PnpResult>(value.estimatedPose);
packet.Pack<std::vector<int16_t>>(value.fiducialIDsUsed);
}

MultiTargetPNPResult StructType::Unpack(Packet& packet) {
return MultiTargetPNPResult{MultiTargetPNPResult_PhotonStruct{
.estimatedPose = packet.Unpack<photon::PnpResult>(),
.fiducialIDsUsed = packet.Unpack<std::vector<int16_t>>(),
}};
}

} // namespace photon
41 changes: 41 additions & 0 deletions src/main/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
/*
* 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/>.
*/

// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO
// NOT MODIFY

#include "photon/serde/PhotonPipelineMetadataSerde.h"

namespace photon {

using StructType = SerdeType<PhotonPipelineMetadata>;

void StructType::Pack(Packet& packet, const PhotonPipelineMetadata& value) {
packet.Pack<int64_t>(value.sequenceID);
packet.Pack<int64_t>(value.captureTimestampMicros);
packet.Pack<int64_t>(value.publishTimestampMicros);
}

PhotonPipelineMetadata StructType::Unpack(Packet& packet) {
return PhotonPipelineMetadata{PhotonPipelineMetadata_PhotonStruct{
.sequenceID = packet.Unpack<int64_t>(),
.captureTimestampMicros = packet.Unpack<int64_t>(),
.publishTimestampMicros = packet.Unpack<int64_t>(),
}};
}

} // namespace photon
Loading

0 comments on commit fdc9431

Please sign in to comment.