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

Multi-tag pnp in robot code (#787)

Browse Source 
---------

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>
Co-authored-by: Joseph Farkas <16584585+MrRedness@users.noreply.github.com>
This commit is contained in:
Matt
2023-02-13 17:57:01 -05:00
committed by GitHub
parent a2dfe48679
commit 5b86360b9b
33 changed files with 1785 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,6 +26,7 @@
#include <frc/Errors.h>
#include <frc/Timer.h>
#include <opencv2/core/mat.hpp>
#include "PhotonVersion.h"
#include "photonlib/Packet.h"
@@ -57,6 +58,10 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
ledModePub(mainTable->GetIntegerTopic("ledMode").Publish()),
ledModeSub(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
versionEntry(mainTable->GetStringTopic("version").Subscribe("")),
cameraIntrinsicsSubscriber(
rootTable->GetDoubleArrayTopic("cameraIntrinsics").Subscribe({})),
cameraDistortionSubscriber(
rootTable->GetDoubleArrayTopic("cameraDistortion").Subscribe({})),
driverModeSubscriber(
rootTable->GetBooleanTopic("driverMode").Subscribe(false)),
driverModePublisher(
@@ -119,12 +124,26 @@ LEDMode PhotonCamera::GetLEDMode() const {
return static_cast<LEDMode>(static_cast<int>(ledModeSub.Get()));
}
std::optional<cv::Mat> PhotonCamera::GetCameraMatrix() {
auto camCoeffs = cameraIntrinsicsSubscriber.Get();
if (camCoeffs.size() == 9) {
// clone should deal with ownership concerns? not sure
return cv::Mat(3, 3, CV_64FC1, camCoeffs.data()).clone();
}
return std::nullopt;
}
void PhotonCamera::SetLEDMode(LEDMode mode) {
ledModePub.Set(static_cast<double>(static_cast<int>(mode)));
}
const std::string_view PhotonCamera::GetCameraName() const {
return m_cameraName;
std::optional<cv::Mat> PhotonCamera::GetDistCoeffs() {
auto distCoeffs = cameraDistortionSubscriber.Get();
if (distCoeffs.size() == 5) {
// clone should deal with ownership concerns? not sure
return cv::Mat(5, 1, CV_64FC1, distCoeffs.data()).clone();
}
return std::nullopt;
}
void PhotonCamera::VerifyVersion() {