From fdc9431255b381034f214266436c51532d973ff5 Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Tue, 3 Sep 2024 20:05:31 -0400 Subject: [PATCH] update photon --- grab_photon.py | 6 +- src/main/cpp/PhotonVersion.cpp | 12 ++ src/main/cpp/photon/PhotonCamera.cpp | 137 +++++++------- src/main/cpp/photon/PhotonPoseEstimator.cpp | 17 +- .../serde/MultiTargetPNPResultSerde.cpp | 39 ++++ .../serde/PhotonPipelineMetadataSerde.cpp | 41 +++++ .../serde/PhotonPipelineResultSerde.cpp | 43 +++++ .../photon/serde/PhotonTrackedTargetSerde.cpp | 59 ++++++ src/main/cpp/photon/serde/PnpResultSerde.cpp | 45 +++++ .../cpp/photon/serde/TargetCornerSerde.cpp | 39 ++++ .../cpp/photon/simulation/PhotonCameraSim.cpp | 100 +++++------ .../photon/targeting/MultiTargetPNPResult.cpp | 38 +--- src/main/cpp/photon/targeting/PNPResult.cpp | 61 +------ .../photon/targeting/PhotonPipelineResult.cpp | 72 +------- .../photon/targeting/PhotonTrackedTarget.cpp | 113 +----------- src/main/cpp/str/Camera.cpp | 2 +- src/main/deploy/commit.txt | 2 +- src/main/include/photon/PhotonCamera.h | 2 +- src/main/include/photon/PhotonPoseEstimator.h | 2 +- .../include/photon/PhotonTargetSortMode.h | 2 +- src/main/include/photon/PhotonUtils.h | 2 +- .../photon/dataflow/structures/Packet.h | 169 ++++++++++++++---- .../include/photon/estimation/OpenCVHelp.h | 39 +++- .../photon/estimation/VisionEstimation.h | 53 +++--- .../photon/serde/MultiTargetPNPResultSerde.h | 53 ++++++ .../serde/PhotonPipelineMetadataSerde.h | 52 ++++++ .../photon/serde/PhotonPipelineResultSerde.h | 57 ++++++ .../photon/serde/PhotonTrackedTargetSerde.h | 58 ++++++ .../include/photon/serde/PnpResultSerde.h | 52 ++++++ .../include/photon/serde/TargetCornerSerde.h | 50 ++++++ .../photon/simulation/PhotonCameraSim.h | 2 +- .../struct/MultiTargetPNPResultStruct.h | 40 +++++ .../struct/PhotonPipelineMetadataStruct.h | 37 ++++ .../struct/PhotonPipelineResultStruct.h | 44 +++++ .../photon/struct/PhotonTrackedTargetStruct.h | 52 ++++++ .../include/photon/struct/PnpResultStruct.h | 41 +++++ .../photon/struct/TargetCornerStruct.h | 36 ++++ .../photon/targeting/MultiTargetPNPResult.h | 22 ++- src/main/include/photon/targeting/PNPResult.h | 27 ++- .../photon/targeting/PhotonPipelineMetadata.h | 40 +++++ .../photon/targeting/PhotonPipelineResult.h | 89 ++++----- .../photon/targeting/PhotonTrackedTarget.h | 57 ++---- .../include/photon/targeting/TargetCorner.h | 38 ++++ src/main/include/str/Vision.h | 2 +- 44 files changed, 1343 insertions(+), 601 deletions(-) create mode 100644 src/main/cpp/PhotonVersion.cpp create mode 100644 src/main/cpp/photon/serde/MultiTargetPNPResultSerde.cpp create mode 100644 src/main/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp create mode 100644 src/main/cpp/photon/serde/PhotonPipelineResultSerde.cpp create mode 100644 src/main/cpp/photon/serde/PhotonTrackedTargetSerde.cpp create mode 100644 src/main/cpp/photon/serde/PnpResultSerde.cpp create mode 100644 src/main/cpp/photon/serde/TargetCornerSerde.cpp create mode 100644 src/main/include/photon/serde/MultiTargetPNPResultSerde.h create mode 100644 src/main/include/photon/serde/PhotonPipelineMetadataSerde.h create mode 100644 src/main/include/photon/serde/PhotonPipelineResultSerde.h create mode 100644 src/main/include/photon/serde/PhotonTrackedTargetSerde.h create mode 100644 src/main/include/photon/serde/PnpResultSerde.h create mode 100644 src/main/include/photon/serde/TargetCornerSerde.h create mode 100644 src/main/include/photon/struct/MultiTargetPNPResultStruct.h create mode 100644 src/main/include/photon/struct/PhotonPipelineMetadataStruct.h create mode 100644 src/main/include/photon/struct/PhotonPipelineResultStruct.h create mode 100644 src/main/include/photon/struct/PhotonTrackedTargetStruct.h create mode 100644 src/main/include/photon/struct/PnpResultStruct.h create mode 100644 src/main/include/photon/struct/TargetCornerStruct.h create mode 100644 src/main/include/photon/targeting/PhotonPipelineMetadata.h create mode 100644 src/main/include/photon/targeting/TargetCorner.h diff --git a/grab_photon.py b/grab_photon.py index 5834c87..3882445 100644 --- a/grab_photon.py +++ b/grab_photon.py @@ -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", diff --git a/src/main/cpp/PhotonVersion.cpp b/src/main/cpp/PhotonVersion.cpp new file mode 100644 index 0000000..47ee7c4 --- /dev/null +++ b/src/main/cpp/PhotonVersion.cpp @@ -0,0 +1,12 @@ +#include +#include + +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; + } +} \ No newline at end of file diff --git a/src/main/cpp/photon/PhotonCamera.cpp b/src/main/cpp/photon/PhotonCamera.cpp index 8841c26..34905a3 100644 --- a/src/main/cpp/photon/PhotonCamera.cpp +++ b/src/main/cpp/photon/PhotonCamera.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #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(); - result.SetRecieveTimestamp(now); + result.SetReceiveTimestamp(now); return result; } @@ -149,8 +148,9 @@ std::vector PhotonCamera::GetAllUnreadResults() { const auto changes = rawBytesEntry.ReadQueue(); - // Create the new result list -- these will be updated in-place - std::vector ret(changes.size()); + // Create the new result list + std::vector ret; + ret.reserve(changes.size()); for (size_t i = 0; i < changes.size(); i++) { const nt::Timestamped>& value = changes[i]; @@ -161,13 +161,14 @@ std::vector PhotonCamera::GetAllUnreadResults() { // Fill the packet with latest data and populate result. photon::Packet packet{value.value}; + auto result = packet.Unpack(); - 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::GetCameraMatrix() { auto camCoeffs = cameraIntrinsicsSubscriber.Get(); if (camCoeffs.size() == 9) { PhotonCamera::CameraMatrix retVal = - Eigen::Map(camCoeffs.data()); + Eigen::Map>( + camCoeffs.data()); return retVal; } + return std::nullopt; } @@ -230,67 +233,57 @@ std::optional 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 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 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::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 PhotonCamera::tablesThatLookLikePhotonCameras() { diff --git a/src/main/cpp/photon/PhotonPoseEstimator.cpp b/src/main/cpp/photon/PhotonPoseEstimator.cpp index 41e577e..64c1512 100644 --- a/src/main/cpp/photon/PhotonPoseEstimator.cpp +++ b/src/main/cpp/photon/PhotonPoseEstimator.cpp @@ -100,6 +100,8 @@ std::optional PhotonPoseEstimator::Update( std::optional 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 PhotonPoseEstimator::Update( } if (ret) { - lastPose = ret.value().estimatedPose; + lastPose = ret->estimatedPose; } return ret; } @@ -197,8 +199,7 @@ std::optional 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 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 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]); } } diff --git a/src/main/cpp/photon/serde/MultiTargetPNPResultSerde.cpp b/src/main/cpp/photon/serde/MultiTargetPNPResultSerde.cpp new file mode 100644 index 0000000..d4b1e77 --- /dev/null +++ b/src/main/cpp/photon/serde/MultiTargetPNPResultSerde.cpp @@ -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 . + */ + +// 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; + +void StructType::Pack(Packet& packet, const MultiTargetPNPResult& value) { + packet.Pack(value.estimatedPose); + packet.Pack>(value.fiducialIDsUsed); +} + +MultiTargetPNPResult StructType::Unpack(Packet& packet) { + return MultiTargetPNPResult{MultiTargetPNPResult_PhotonStruct{ + .estimatedPose = packet.Unpack(), + .fiducialIDsUsed = packet.Unpack>(), + }}; +} + +} // namespace photon diff --git a/src/main/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp b/src/main/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp new file mode 100644 index 0000000..e5b44f1 --- /dev/null +++ b/src/main/cpp/photon/serde/PhotonPipelineMetadataSerde.cpp @@ -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 . + */ + +// 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; + +void StructType::Pack(Packet& packet, const PhotonPipelineMetadata& value) { + packet.Pack(value.sequenceID); + packet.Pack(value.captureTimestampMicros); + packet.Pack(value.publishTimestampMicros); +} + +PhotonPipelineMetadata StructType::Unpack(Packet& packet) { + return PhotonPipelineMetadata{PhotonPipelineMetadata_PhotonStruct{ + .sequenceID = packet.Unpack(), + .captureTimestampMicros = packet.Unpack(), + .publishTimestampMicros = packet.Unpack(), + }}; +} + +} // namespace photon diff --git a/src/main/cpp/photon/serde/PhotonPipelineResultSerde.cpp b/src/main/cpp/photon/serde/PhotonPipelineResultSerde.cpp new file mode 100644 index 0000000..a1f7d50 --- /dev/null +++ b/src/main/cpp/photon/serde/PhotonPipelineResultSerde.cpp @@ -0,0 +1,43 @@ +/* + * 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 . + */ + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +#include "photon/serde/PhotonPipelineResultSerde.h" + +namespace photon { + +using StructType = SerdeType; + +void StructType::Pack(Packet& packet, const PhotonPipelineResult& value) { + packet.Pack(value.metadata); + packet.Pack>(value.targets); + packet.Pack>( + value.multitagResult); +} + +PhotonPipelineResult StructType::Unpack(Packet& packet) { + return PhotonPipelineResult{PhotonPipelineResult_PhotonStruct{ + .metadata = packet.Unpack(), + .targets = packet.Unpack>(), + .multitagResult = + packet.Unpack>(), + }}; +} + +} // namespace photon diff --git a/src/main/cpp/photon/serde/PhotonTrackedTargetSerde.cpp b/src/main/cpp/photon/serde/PhotonTrackedTargetSerde.cpp new file mode 100644 index 0000000..6b6b400 --- /dev/null +++ b/src/main/cpp/photon/serde/PhotonTrackedTargetSerde.cpp @@ -0,0 +1,59 @@ +/* + * 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 . + */ + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +#include "photon/serde/PhotonTrackedTargetSerde.h" + +namespace photon { + +using StructType = SerdeType; + +void StructType::Pack(Packet& packet, const PhotonTrackedTarget& value) { + packet.Pack(value.yaw); + packet.Pack(value.pitch); + packet.Pack(value.area); + packet.Pack(value.skew); + packet.Pack(value.fiducialId); + packet.Pack(value.objDetectId); + packet.Pack(value.objDetectConf); + packet.Pack(value.bestCameraToTarget); + packet.Pack(value.altCameraToTarget); + packet.Pack(value.poseAmbiguity); + packet.Pack>(value.minAreaRectCorners); + packet.Pack>(value.detectedCorners); +} + +PhotonTrackedTarget StructType::Unpack(Packet& packet) { + return PhotonTrackedTarget{PhotonTrackedTarget_PhotonStruct{ + .yaw = packet.Unpack(), + .pitch = packet.Unpack(), + .area = packet.Unpack(), + .skew = packet.Unpack(), + .fiducialId = packet.Unpack(), + .objDetectId = packet.Unpack(), + .objDetectConf = packet.Unpack(), + .bestCameraToTarget = packet.Unpack(), + .altCameraToTarget = packet.Unpack(), + .poseAmbiguity = packet.Unpack(), + .minAreaRectCorners = packet.Unpack>(), + .detectedCorners = packet.Unpack>(), + }}; +} + +} // namespace photon diff --git a/src/main/cpp/photon/serde/PnpResultSerde.cpp b/src/main/cpp/photon/serde/PnpResultSerde.cpp new file mode 100644 index 0000000..521da3e --- /dev/null +++ b/src/main/cpp/photon/serde/PnpResultSerde.cpp @@ -0,0 +1,45 @@ +/* + * 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 . + */ + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +#include "photon/serde/PnpResultSerde.h" + +namespace photon { + +using StructType = SerdeType; + +void StructType::Pack(Packet& packet, const PnpResult& value) { + packet.Pack(value.best); + packet.Pack(value.alt); + packet.Pack(value.bestReprojErr); + packet.Pack(value.altReprojErr); + packet.Pack(value.ambiguity); +} + +PnpResult StructType::Unpack(Packet& packet) { + return PnpResult{PnpResult_PhotonStruct{ + .best = packet.Unpack(), + .alt = packet.Unpack(), + .bestReprojErr = packet.Unpack(), + .altReprojErr = packet.Unpack(), + .ambiguity = packet.Unpack(), + }}; +} + +} // namespace photon diff --git a/src/main/cpp/photon/serde/TargetCornerSerde.cpp b/src/main/cpp/photon/serde/TargetCornerSerde.cpp new file mode 100644 index 0000000..f492bdb --- /dev/null +++ b/src/main/cpp/photon/serde/TargetCornerSerde.cpp @@ -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 . + */ + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +#include "photon/serde/TargetCornerSerde.h" + +namespace photon { + +using StructType = SerdeType; + +void StructType::Pack(Packet& packet, const TargetCorner& value) { + packet.Pack(value.x); + packet.Pack(value.y); +} + +TargetCorner StructType::Unpack(Packet& packet) { + return TargetCorner{TargetCorner_PhotonStruct{ + .x = packet.Unpack(), + .y = packet.Unpack(), + }}; +} + +} // namespace photon diff --git a/src/main/cpp/photon/simulation/PhotonCameraSim.cpp b/src/main/cpp/photon/simulation/PhotonCameraSim.cpp index 381e99c..94c31f0 100644 --- a/src/main/cpp/photon/simulation/PhotonCameraSim.cpp +++ b/src/main/cpp/photon/simulation/PhotonCameraSim.cpp @@ -201,33 +201,34 @@ PhotonPipelineResult PhotonCameraSim::Process( continue; } - PNPResult pnpSim{}; + std::optional 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> tempCorners = OpenCVHelp::PointsToCorners(minAreaRectPts); - wpi::SmallVector, 4> smallVec; + std::vector smallVec; for (const auto& corner : tempCorners) { - smallVec.emplace_back(std::make_pair(static_cast(corner.first), - static_cast(corner.second))); + smallVec.emplace_back(static_cast(corner.first), + static_cast(corner.second)); } - std::vector> cornersFloat = - OpenCVHelp::PointsToCorners(noisyTargetCorners); + auto cornersFloat = OpenCVHelp::PointsToTargetCorners(noisyTargetCorners); - std::vector> cornersDouble{cornersFloat.begin(), - cornersFloat.end()}; + std::vector cornersDouble{cornersFloat.begin(), + cornersFloat.end()}; detectableTgts.emplace_back( -centerRot.Z().convert().to(), -centerRot.Y().convert().to(), areaPercent, centerRot.X().convert().to(), 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> 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(VideoSimUtil::GetScaledThickness( 1, videoSimFrameProcessed)), cv::LINE_AA); - wpi::SmallVector, 4> smallVec = - tgt.GetMinAreaRectCorners(); + auto smallVec = tgt.GetMinAreaRectCorners(); std::vector> cornersCopy{}; cornersCopy.reserve(4); - for (const auto& corner : smallVec) { - cornersCopy.emplace_back( - std::make_pair(static_cast(corner.first), - static_cast(corner.second))); - } - VideoSimUtil::DrawPoly( - OpenCVHelp::CornersToPoints(cornersCopy), + OpenCVHelp::CornersToPoints(smallVec), static_cast( 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 multiTagResults = std::nullopt; std::vector visibleLayoutTags = VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout); if (visibleLayoutTags.size() > 1) { - wpi::SmallVector usedIds{}; + std::vector 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()}, + 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().to(), - 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 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 poseData{ transform.X().to(), transform.Y().to(), transform.Rotation().ToRotation2d().Degrees().to()}; - ts.targetPoseEntry.Set(poseData, recieveTimestamp); + ts.targetPoseEntry.Set(poseData, ReceiveTimestamp); } - auto intrinsics = prop.GetIntrinsics(); - std::vector intrinsicsView{intrinsics.data(), - intrinsics.data() + intrinsics.size()}; - ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp); + Eigen::Matrix intrinsics = + prop.GetIntrinsics(); + std::span intrinsicsView{intrinsics.data(), + intrinsics.data() + intrinsics.size()}; + ts.cameraIntrinsicsPublisher.Set(intrinsicsView, ReceiveTimestamp); auto distortion = prop.GetDistCoeffs(); std::vector 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(); } diff --git a/src/main/cpp/photon/targeting/MultiTargetPNPResult.cpp b/src/main/cpp/photon/targeting/MultiTargetPNPResult.cpp index 3cfdbfe..fae08ed 100644 --- a/src/main/cpp/photon/targeting/MultiTargetPNPResult.cpp +++ b/src/main/cpp/photon/targeting/MultiTargetPNPResult.cpp @@ -17,40 +17,4 @@ #include "photon/targeting/MultiTargetPNPResult.h" -namespace photon { - -bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other) const { - return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed; -} - -Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result) { - packet << result.result; - - size_t i; - for (i = 0; i < result.fiducialIdsUsed.capacity(); i++) { - if (i < result.fiducialIdsUsed.size()) { - packet << static_cast(result.fiducialIdsUsed[i]); - } else { - packet << static_cast(-1); - } - } - - return packet; -} - -Packet& operator>>(Packet& packet, MultiTargetPNPResult& result) { - packet >> result.result; - - result.fiducialIdsUsed.clear(); - for (size_t i = 0; i < result.fiducialIdsUsed.capacity(); i++) { - int16_t id = 0; - packet >> id; - - if (id > -1) { - result.fiducialIdsUsed.push_back(id); - } - } - - return packet; -} -} // namespace photon +namespace photon {} // namespace photon diff --git a/src/main/cpp/photon/targeting/PNPResult.cpp b/src/main/cpp/photon/targeting/PNPResult.cpp index b43e434..4d87374 100644 --- a/src/main/cpp/photon/targeting/PNPResult.cpp +++ b/src/main/cpp/photon/targeting/PNPResult.cpp @@ -15,63 +15,6 @@ * along with this program. If not, see . */ -#include "photon/targeting/PNPResult.h" +#include "photon/targeting/PnpResult.h" -namespace photon { -bool PNPResult::operator==(const PNPResult& other) const { - return other.isPresent == isPresent && other.best == best && - other.bestReprojErr == bestReprojErr && other.alt == alt && - other.altReprojErr == altReprojErr && other.ambiguity == ambiguity; -} - -// Encode a transform3d -Packet& operator<<(Packet& packet, const frc::Transform3d& transform) { - packet << transform.Translation().X().value() - << transform.Translation().Y().value() - << transform.Translation().Z().value() - << transform.Rotation().GetQuaternion().W() - << transform.Rotation().GetQuaternion().X() - << transform.Rotation().GetQuaternion().Y() - << transform.Rotation().GetQuaternion().Z(); - - return packet; -} - -// Decode a transform3d -Packet& operator>>(Packet& packet, frc::Transform3d& transform) { - frc::Transform3d ret; - - // We use these for best and alt transforms below - double x = 0; - double y = 0; - double z = 0; - double w = 0; - - // decode and unitify translation - packet >> x >> y >> z; - const auto translation = frc::Translation3d( - units::meter_t(x), units::meter_t(y), units::meter_t(z)); - - // decode and add units to rotation - packet >> w >> x >> y >> z; - const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); - - transform = frc::Transform3d(translation, rotation); - - return packet; -} - -Packet& operator<<(Packet& packet, PNPResult const& result) { - packet << result.isPresent << result.best << result.alt - << result.bestReprojErr << result.altReprojErr << result.ambiguity; - - return packet; -} - -Packet& operator>>(Packet& packet, PNPResult& result) { - packet >> result.isPresent >> result.best >> result.alt >> - result.bestReprojErr >> result.altReprojErr >> result.ambiguity; - - return packet; -} -} // namespace photon +namespace photon {} // namespace photon diff --git a/src/main/cpp/photon/targeting/PhotonPipelineResult.cpp b/src/main/cpp/photon/targeting/PhotonPipelineResult.cpp index c94e55b..b6f7c6c 100644 --- a/src/main/cpp/photon/targeting/PhotonPipelineResult.cpp +++ b/src/main/cpp/photon/targeting/PhotonPipelineResult.cpp @@ -17,74 +17,4 @@ #include "photon/targeting/PhotonPipelineResult.h" -#include - -namespace photon { -PhotonPipelineResult::PhotonPipelineResult( - int64_t sequenceID, units::microsecond_t captureTimestamp, - units::microsecond_t publishTimestamp, - std::span targets, - MultiTargetPNPResult multitagResult) - : sequenceID(sequenceID), - captureTimestamp(captureTimestamp), - publishTimestamp(publishTimestamp), - targets(targets.data(), targets.data() + targets.size()), - multitagResult(multitagResult) {} - -bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { - return sequenceID == other.sequenceID && - captureTimestamp == other.captureTimestamp && - publishTimestamp == other.publishTimestamp && - ntRecieveTimestamp == other.ntRecieveTimestamp && - targets == other.targets && multitagResult == other.multitagResult; -} - -Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { - // Encode latency and number of targets. - packet << result.sequenceID - << static_cast(result.captureTimestamp.value()) - << static_cast(result.publishTimestamp.value()) - << static_cast(result.targets.size()); - - // Encode the information of each target. - for (auto& target : result.targets) packet << target; - - packet << result.multitagResult; - // Return the packet - return packet; -} - -Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { - // Decode latency, existence of targets, and number of targets. - int64_t sequenceID = 0; - int64_t capTS = 0; - int64_t pubTS = 0; - int8_t targetCount = 0; - std::vector targets; - MultiTargetPNPResult multitagResult; - - packet >> sequenceID >> capTS >> pubTS >> targetCount; - - targets.clear(); - targets.reserve(targetCount); - - // Decode the information of each target. - for (int i = 0; i < targetCount; ++i) { - PhotonTrackedTarget target; - packet >> target; - targets.push_back(target); - } - - packet >> multitagResult; - - units::microsecond_t captureTS = - units::microsecond_t{static_cast(capTS)}; - units::microsecond_t publishTS = - units::microsecond_t{static_cast(pubTS)}; - - result = PhotonPipelineResult{sequenceID, captureTS, publishTS, targets, - multitagResult}; - - return packet; -} -} // namespace photon +namespace photon {} // namespace photon diff --git a/src/main/cpp/photon/targeting/PhotonTrackedTarget.cpp b/src/main/cpp/photon/targeting/PhotonTrackedTarget.cpp index 6a4e367..636964c 100644 --- a/src/main/cpp/photon/targeting/PhotonTrackedTarget.cpp +++ b/src/main/cpp/photon/targeting/PhotonTrackedTarget.cpp @@ -27,115 +27,4 @@ static constexpr const uint8_t MAX_CORNERS = 8; -namespace photon { - -PhotonTrackedTarget::PhotonTrackedTarget( - double yaw, double pitch, double area, double skew, int id, int objdetid, - float objdetconf, const frc::Transform3d& pose, - const frc::Transform3d& alternatePose, double ambiguity, - const wpi::SmallVector, 4> minAreaRectCorners, - const std::vector> detectedCorners) - : yaw(yaw), - pitch(pitch), - area(area), - skew(skew), - fiducialId(id), - objDetectId(objdetid), - objDetectConf(objdetconf), - bestCameraToTarget(pose), - altCameraToTarget(alternatePose), - poseAmbiguity(ambiguity), - minAreaRectCorners(minAreaRectCorners), - detectedCorners(detectedCorners) {} - -bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const { - return other.yaw == yaw && other.pitch == pitch && other.area == area && - other.skew == skew && other.bestCameraToTarget == bestCameraToTarget && - other.objDetectConf == objDetectConf && - other.objDetectId == objDetectId && - other.minAreaRectCorners == minAreaRectCorners; -} - -Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) { - packet << target.yaw << target.pitch << target.area << target.skew - << target.fiducialId << target.objDetectId << target.objDetectConf - << target.bestCameraToTarget.Translation().X().value() - << target.bestCameraToTarget.Translation().Y().value() - << target.bestCameraToTarget.Translation().Z().value() - << target.bestCameraToTarget.Rotation().GetQuaternion().W() - << target.bestCameraToTarget.Rotation().GetQuaternion().X() - << target.bestCameraToTarget.Rotation().GetQuaternion().Y() - << target.bestCameraToTarget.Rotation().GetQuaternion().Z() - << target.altCameraToTarget.Translation().X().value() - << target.altCameraToTarget.Translation().Y().value() - << target.altCameraToTarget.Translation().Z().value() - << target.altCameraToTarget.Rotation().GetQuaternion().W() - << target.altCameraToTarget.Rotation().GetQuaternion().X() - << target.altCameraToTarget.Rotation().GetQuaternion().Y() - << target.altCameraToTarget.Rotation().GetQuaternion().Z() - << target.poseAmbiguity; - - for (int i = 0; i < 4; i++) { - packet << target.minAreaRectCorners[i].first - << target.minAreaRectCorners[i].second; - } - - uint8_t num_corners = - std::min(target.detectedCorners.size(), MAX_CORNERS); - packet << num_corners; - for (size_t i = 0; i < target.detectedCorners.size(); i++) { - packet << target.detectedCorners[i].first - << target.detectedCorners[i].second; - } - - return packet; -} - -Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) { - packet >> target.yaw >> target.pitch >> target.area >> target.skew >> - target.fiducialId >> target.objDetectId >> target.objDetectConf; - - // We use these for best and alt transforms below - double x = 0; - double y = 0; - double z = 0; - double w = 0; - - // First transform is the "best" pose - packet >> x >> y >> z; - const auto bestTranslation = frc::Translation3d( - units::meter_t(x), units::meter_t(y), units::meter_t(z)); - packet >> w >> x >> y >> z; - const auto bestRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); - target.bestCameraToTarget = frc::Transform3d(bestTranslation, bestRotation); - - // Second transform is the "alternate" pose - packet >> x >> y >> z; - const auto altTranslation = frc::Translation3d( - units::meter_t(x), units::meter_t(y), units::meter_t(z)); - packet >> w >> x >> y >> z; - const auto altRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); - target.altCameraToTarget = frc::Transform3d(altTranslation, altRotation); - - packet >> target.poseAmbiguity; - - target.minAreaRectCorners.clear(); - double first = 0; - double second = 0; - for (int i = 0; i < 4; i++) { - packet >> first >> second; - target.minAreaRectCorners.emplace_back(first, second); - } - - uint8_t numCorners = 0; - packet >> numCorners; - target.detectedCorners.clear(); - target.detectedCorners.reserve(numCorners); - for (size_t i = 0; i < numCorners; i++) { - packet >> first >> second; - target.detectedCorners.emplace_back(first, second); - } - - return packet; -} -} // namespace photon +namespace photon {} // namespace photon diff --git a/src/main/cpp/str/Camera.cpp b/src/main/cpp/str/Camera.cpp index b9f21dc..4bf8a4a 100644 --- a/src/main/cpp/str/Camera.cpp +++ b/src/main/cpp/str/Camera.cpp @@ -35,7 +35,7 @@ Camera::Camera(std::string cameraName, frc::Transform3d robotToCamera, visionSim->AddAprilTags(consts::yearSpecific::aprilTagLayout); cameraProps = std::make_unique(); - cameraProps->SetCalibration(1600, 1200, frc::Rotation2d{90_deg}); + cameraProps->SetCalibration(1600, 1200, frc::Rotation2d{75_deg}); cameraProps->SetCalibError(.35, .10); cameraProps->SetFPS(45_Hz); cameraProps->SetAvgLatency(20_ms); diff --git a/src/main/deploy/commit.txt b/src/main/deploy/commit.txt index 3d91df7..46612e9 100644 --- a/src/main/deploy/commit.txt +++ b/src/main/deploy/commit.txt @@ -1 +1 @@ -93bc473 \ No newline at end of file +3cae4b3 \ No newline at end of file diff --git a/src/main/include/photon/PhotonCamera.h b/src/main/include/photon/PhotonCamera.h index e1e5f35..e03de9c 100644 --- a/src/main/include/photon/PhotonCamera.h +++ b/src/main/include/photon/PhotonCamera.h @@ -39,7 +39,7 @@ #include #include -#include "photon/targeting//PhotonPipelineResult.h" +#include "photon/targeting/PhotonPipelineResult.h" namespace cv { class Mat; diff --git a/src/main/include/photon/PhotonPoseEstimator.h b/src/main/include/photon/PhotonPoseEstimator.h index 37e3822..435334b 100644 --- a/src/main/include/photon/PhotonPoseEstimator.h +++ b/src/main/include/photon/PhotonPoseEstimator.h @@ -34,9 +34,9 @@ #include "photon/PhotonCamera.h" #include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting/PnpResult.h" namespace photon { enum PoseStrategy { diff --git a/src/main/include/photon/PhotonTargetSortMode.h b/src/main/include/photon/PhotonTargetSortMode.h index c8ad53c..b2a7b4f 100644 --- a/src/main/include/photon/PhotonTargetSortMode.h +++ b/src/main/include/photon/PhotonTargetSortMode.h @@ -28,9 +28,9 @@ #include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting/PnpResult.h" namespace photon { diff --git a/src/main/include/photon/PhotonUtils.h b/src/main/include/photon/PhotonUtils.h index 14076a5..5ea28eb 100644 --- a/src/main/include/photon/PhotonUtils.h +++ b/src/main/include/photon/PhotonUtils.h @@ -34,9 +34,9 @@ #include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" #include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting/PnpResult.h" namespace photon { class PhotonUtils { diff --git a/src/main/include/photon/dataflow/structures/Packet.h b/src/main/include/photon/dataflow/structures/Packet.h index 242aa9d..b36a818 100644 --- a/src/main/include/photon/dataflow/structures/Packet.h +++ b/src/main/include/photon/dataflow/structures/Packet.h @@ -20,11 +20,46 @@ #include #include #include +#include +#include +#include #include #include +#include +#include +#include + namespace photon { +class Packet; + +// Struct is where all our actual ser/de methods are implemented +template +struct SerdeType {}; + +template +concept PhotonStructSerializable = requires(Packet& packet, const T& value) { + typename SerdeType>; + + // MD6sum of the message definition + { + SerdeType>::GetSchemaHash() + } -> std::convertible_to; + // JSON-encoded message chema + { + SerdeType>::GetSchema() + } -> std::convertible_to; + // Unpack myself from a packet + { + SerdeType>::Unpack(packet) + } -> std::same_as>; + // Pack myself into a packet + { + SerdeType>::Pack(packet, value) + } -> std::same_as; +}; + /** * A packet that holds byte-packed data to be sent over NetworkTables. */ @@ -33,7 +68,7 @@ class Packet { /** * Constructs an empty packet. */ - Packet() = default; + explicit Packet(int initialCapacity = 0) : packetData(initialCapacity) {} /** * Constructs a packet with the given data. @@ -58,47 +93,41 @@ class Packet { */ inline size_t GetDataSize() const { return packetData.size(); } - /** - * Adds a value to the data buffer. This should only be used with PODs. - * @tparam T The data type. - * @param src The data source. - * @return A reference to the current object. - */ - template - Packet& operator<<(T src) { - packetData.resize(packetData.size() + sizeof(T)); - std::memcpy(packetData.data() + writePos, &src, sizeof(T)); - - if constexpr (std::endian::native == std::endian::little) { - // Reverse to big endian for network conventions. - std::reverse(packetData.data() + writePos, - packetData.data() + writePos + sizeof(T)); - } + template + requires wpi::StructSerializable + inline void Pack(const T& value) { + // as WPI struct stuff assumes constant data length - reserve at least + // enough new space for our new member + size_t newWritePos = writePos + wpi::GetStructSize(); + packetData.resize(newWritePos); - writePos += sizeof(T); - return *this; + wpi::PackStruct( + std::span{packetData.begin() + writePos, packetData.end()}, + value); + + writePos = newWritePos; } - /** - * Extracts a value to the provided destination. - * @tparam T The type of value to extract. - * @param value The value to extract. - * @return A reference to the current object. - */ template - Packet& operator>>(T& value) { - if (!packetData.empty()) { - std::memcpy(&value, packetData.data() + readPos, sizeof(T)); - - if constexpr (std::endian::native == std::endian::little) { - // Reverse to little endian for host. - uint8_t& raw = reinterpret_cast(value); - std::reverse(&raw, &raw + sizeof(T)); - } - } + requires(PhotonStructSerializable) + inline void Pack(const T& value) { + SerdeType>::Pack(*this, value); + } + + template + requires wpi::StructSerializable + inline T Unpack() { + // Unpack this member, starting at readPos + T ret = wpi::UnpackStruct( + std::span{packetData.begin() + readPos, packetData.end()}); + readPos += wpi::GetStructSize(); + return ret; + } - readPos += sizeof(T); - return *this; + template + requires(PhotonStructSerializable) + inline T Unpack() { + return SerdeType>::Unpack(*this); } bool operator==(const Packet& right) const; @@ -106,9 +135,73 @@ class Packet { private: // Data stored in the packet - std::vector packetData; + std::vector packetData{}; size_t readPos = 0; size_t writePos = 0; }; + +template +concept arithmetic = std::integral || std::floating_point; + +// support encoding vectors +template + requires(PhotonStructSerializable || arithmetic) +struct SerdeType> { + static std::vector Unpack(Packet& packet) { + uint8_t len = packet.Unpack(); + std::vector ret; + ret.reserve(len); + for (size_t i = 0; i < len; i++) { + ret.push_back(packet.Unpack()); + } + return ret; + } + static void Pack(Packet& packet, const std::vector& value) { + packet.Pack(value.size()); + for (const auto& thing : value) { + packet.Pack(thing); + } + } + static constexpr std::string_view GetSchemaHash() { + // quick hack lol + return SerdeType::GetSchemaHash(); + } + + static constexpr std::string_view GetSchema() { + // TODO: this gets us the plain type name of T, but this is not schema JSON + // compliant! + return "TODO[?]"; + } +}; + +// support encoding optional types +template + requires(PhotonStructSerializable || arithmetic) +struct SerdeType> { + static std::optional Unpack(Packet& packet) { + if (packet.Unpack() == 1u) { + return packet.Unpack(); + } else { + return std::nullopt; + } + } + static void Pack(Packet& packet, const std::optional& value) { + packet.Pack(value.has_value()); + if (value) { + packet.Pack(*value); + } + } + static constexpr std::string_view GetSchemaHash() { + // quick hack lol + return SerdeType::GetSchemaHash(); + } + + static constexpr std::string_view GetSchema() { + // TODO: this gets us the plain type name of T, but this is not schema JSON + // compliant! + return "TODO?"; + } +}; + } // namespace photon diff --git a/src/main/include/photon/estimation/OpenCVHelp.h b/src/main/include/photon/estimation/OpenCVHelp.h index 120e063..ebb6a9f 100644 --- a/src/main/include/photon/estimation/OpenCVHelp.h +++ b/src/main/include/photon/estimation/OpenCVHelp.h @@ -29,9 +29,11 @@ #define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT #include -#include "photon/targeting/PNPResult.h" +#include "photon/targeting/PnpResult.h" #include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/TargetCorner.h" + namespace photon { namespace OpenCVHelp { @@ -96,6 +98,16 @@ static std::vector RotationToRVec( return points[0]; } +[[maybe_unused]] static std::vector PointsToTargetCorners( + const std::vector& points) { + std::vector retVal; + retVal.reserve(points.size()); + for (size_t i = 0; i < points.size(); i++) { + retVal.emplace_back(photon::TargetCorner{points[i].x, points[i].y}); + } + return retVal; +} + [[maybe_unused]] static std::vector> PointsToCorners( const std::vector& points) { std::vector> retVal; @@ -116,6 +128,17 @@ static std::vector RotationToRVec( return retVal; } +[[maybe_unused]] static std::vector CornersToPoints( + const std::vector& corners) { + std::vector retVal; + retVal.reserve(corners.size()); + for (size_t i = 0; i < corners.size(); i++) { + retVal.emplace_back(cv::Point2f{static_cast(corners[i].x), + static_cast(corners[i].y)}); + } + return retVal; +} + [[maybe_unused]] static cv::Rect GetBoundingRect( const std::vector& points) { return cv::boundingRect(points); @@ -184,7 +207,7 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { units::radian_t{data[2]}}); } -[[maybe_unused]] static photon::PNPResult SolvePNP_Square( +[[maybe_unused]] static std::optional SolvePNP_Square( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, std::vector modelTrls, @@ -233,26 +256,25 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { if (std::isnan(errors[0])) { fmt::print("SolvePNP_Square failed!\n"); + return std::nullopt; } if (alt) { - photon::PNPResult result; + photon::PnpResult result; result.best = best; result.alt = alt.value(); result.ambiguity = errors[0] / errors[1]; result.bestReprojErr = errors[0]; result.altReprojErr = errors[1]; - result.isPresent = true; return result; } else { - photon::PNPResult result; + photon::PnpResult result; result.best = best; result.bestReprojErr = errors[0]; - result.isPresent = true; return result; } } -[[maybe_unused]] static photon::PNPResult SolvePNP_SQPNP( +[[maybe_unused]] static std::optional SolvePNP_SQPNP( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, std::vector modelTrls, @@ -283,10 +305,9 @@ static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { if (std::isnan(error)) { fmt::print("SolvePNP_Square failed!\n"); } - photon::PNPResult result; + photon::PnpResult result; result.best = best; result.bestReprojErr = error; - result.isPresent = true; return result; } } // namespace OpenCVHelp diff --git a/src/main/include/photon/estimation/VisionEstimation.h b/src/main/include/photon/estimation/VisionEstimation.h index 6121342..8924e5d 100644 --- a/src/main/include/photon/estimation/VisionEstimation.h +++ b/src/main/include/photon/estimation/VisionEstimation.h @@ -47,16 +47,18 @@ static std::vector GetVisibleLayoutTags( return retVal; } -static PNPResult EstimateCamPosePNP( +#include + +static std::optional EstimateCamPosePNP( const Eigen::Matrix& cameraMatrix, const Eigen::Matrix& distCoeffs, const std::vector& visTags, const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) { if (visTags.size() == 0) { - return PNPResult(); + return PnpResult(); } - std::vector> corners{}; + std::vector corners{}; std::vector knownTags{}; for (const auto& tgt : visTags) { @@ -70,30 +72,30 @@ static PNPResult EstimateCamPosePNP( } } if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { - return PNPResult{}; + return PnpResult{}; } std::vector points = OpenCVHelp::CornersToPoints(corners); if (knownTags.size() == 1) { - PNPResult camToTag = OpenCVHelp::SolvePNP_Square( - cameraMatrix, distCoeffs, tagModel.GetVertices(), points); - if (!camToTag.isPresent) { - return PNPResult{}; + auto camToTag = OpenCVHelp::SolvePNP_Square(cameraMatrix, distCoeffs, + tagModel.GetVertices(), points); + if (!camToTag) { + return PnpResult{}; } frc::Pose3d bestPose = - knownTags[0].pose.TransformBy(camToTag.best.Inverse()); + knownTags[0].pose.TransformBy(camToTag->best.Inverse()); frc::Pose3d altPose{}; - if (camToTag.ambiguity != 0) { - altPose = knownTags[0].pose.TransformBy(camToTag.alt.Inverse()); + if (camToTag->ambiguity != 0) { + altPose = knownTags[0].pose.TransformBy(camToTag->alt.Inverse()); } frc::Pose3d o{}; - PNPResult result{}; + PnpResult result{}; result.best = frc::Transform3d{o, bestPose}; result.alt = frc::Transform3d{o, altPose}; - result.ambiguity = camToTag.ambiguity; - result.bestReprojErr = camToTag.bestReprojErr; - result.altReprojErr = camToTag.altReprojErr; + result.ambiguity = camToTag->ambiguity; + result.bestReprojErr = camToTag->bestReprojErr; + result.altReprojErr = camToTag->altReprojErr; return result; } else { std::vector objectTrls{}; @@ -101,20 +103,15 @@ static PNPResult EstimateCamPosePNP( auto verts = tagModel.GetFieldVertices(tag.pose); objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); } - PNPResult camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, - objectTrls, points); - if (!camToOrigin.isPresent) { - return PNPResult{}; - } else { - PNPResult result{}; - result.best = camToOrigin.best.Inverse(), - result.alt = camToOrigin.alt.Inverse(), - result.ambiguity = camToOrigin.ambiguity; - result.bestReprojErr = camToOrigin.bestReprojErr; - result.altReprojErr = camToOrigin.altReprojErr; - result.isPresent = true; - return result; + auto ret = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, + points); + if (ret) { + // Invert best/alt transforms + ret->best = ret->best.Inverse(); + ret->alt = ret->alt.Inverse(); } + + return ret; } } diff --git a/src/main/include/photon/serde/MultiTargetPNPResultSerde.h b/src/main/include/photon/serde/MultiTargetPNPResultSerde.h new file mode 100644 index 0000000..7a75354 --- /dev/null +++ b/src/main/include/photon/serde/MultiTargetPNPResultSerde.h @@ -0,0 +1,53 @@ +/* + * 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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +#include + +// Include myself +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/MultiTargetPNPResult.h" + +// Includes for dependant types +#include "photon/targeting/PnpResult.h" +#include +#include + +namespace photon { + +template <> +struct WPILIB_DLLEXPORT SerdeType { + static constexpr std::string_view GetSchemaHash() { + return "ffc1cb847deb6e796a583a5b1885496b"; + } + + static constexpr std::string_view GetSchema() { + return "PnpResult estimatedPose;int16[?] fiducialIDsUsed;"; + } + + static photon::MultiTargetPNPResult Unpack(photon::Packet& packet); + static void Pack(photon::Packet& packet, + const photon::MultiTargetPNPResult& value); +}; + +static_assert(photon::PhotonStructSerializable); + +} // namespace photon diff --git a/src/main/include/photon/serde/PhotonPipelineMetadataSerde.h b/src/main/include/photon/serde/PhotonPipelineMetadataSerde.h new file mode 100644 index 0000000..d388360 --- /dev/null +++ b/src/main/include/photon/serde/PhotonPipelineMetadataSerde.h @@ -0,0 +1,52 @@ +/* + * 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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +#include + +// Include myself +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/PhotonPipelineMetadata.h" + +// Includes for dependant types +#include + +namespace photon { + +template <> +struct WPILIB_DLLEXPORT SerdeType { + static constexpr std::string_view GetSchemaHash() { + return "2a7039527bda14d13028a1b9282d40a2"; + } + + static constexpr std::string_view GetSchema() { + return "int64 sequenceID;int64 captureTimestampMicros;int64 " + "publishTimestampMicros;"; + } + + static photon::PhotonPipelineMetadata Unpack(photon::Packet& packet); + static void Pack(photon::Packet& packet, + const photon::PhotonPipelineMetadata& value); +}; + +static_assert(photon::PhotonStructSerializable); + +} // namespace photon diff --git a/src/main/include/photon/serde/PhotonPipelineResultSerde.h b/src/main/include/photon/serde/PhotonPipelineResultSerde.h new file mode 100644 index 0000000..b7872d0 --- /dev/null +++ b/src/main/include/photon/serde/PhotonPipelineResultSerde.h @@ -0,0 +1,57 @@ +/* + * 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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +#include + +// Include myself +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/PhotonPipelineResult.h" + +// Includes for dependant types +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PhotonPipelineMetadata.h" +#include "photon/targeting/PhotonTrackedTarget.h" +#include +#include +#include + +namespace photon { + +template <> +struct WPILIB_DLLEXPORT SerdeType { + static constexpr std::string_view GetSchemaHash() { + return "cb3e1605048ba49325888eb797399fe2"; + } + + static constexpr std::string_view GetSchema() { + return "PhotonPipelineMetadata metadata;PhotonTrackedTarget[?] " + "targets;MultiTargetPNPResult? multitagResult;"; + } + + static photon::PhotonPipelineResult Unpack(photon::Packet& packet); + static void Pack(photon::Packet& packet, + const photon::PhotonPipelineResult& value); +}; + +static_assert(photon::PhotonStructSerializable); + +} // namespace photon diff --git a/src/main/include/photon/serde/PhotonTrackedTargetSerde.h b/src/main/include/photon/serde/PhotonTrackedTargetSerde.h new file mode 100644 index 0000000..abc2196 --- /dev/null +++ b/src/main/include/photon/serde/PhotonTrackedTargetSerde.h @@ -0,0 +1,58 @@ +/* + * 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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +#include + +// Include myself +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/PhotonTrackedTarget.h" + +// Includes for dependant types +#include "photon/targeting/TargetCorner.h" +#include +#include +#include + +namespace photon { + +template <> +struct WPILIB_DLLEXPORT SerdeType { + static constexpr std::string_view GetSchemaHash() { + return "8fdada56b9162f2e32bd24f0055d7b60"; + } + + static constexpr std::string_view GetSchema() { + return "float64 yaw;float64 pitch;float64 area;float64 skew;int32 " + "fiducialId;int32 objDetectId;float32 objDetectConf;Transform3d " + "bestCameraToTarget;Transform3d altCameraToTarget;float64 " + "poseAmbiguity;TargetCorner[?] minAreaRectCorners;TargetCorner[?] " + "detectedCorners;"; + } + + static photon::PhotonTrackedTarget Unpack(photon::Packet& packet); + static void Pack(photon::Packet& packet, + const photon::PhotonTrackedTarget& value); +}; + +static_assert(photon::PhotonStructSerializable); + +} // namespace photon diff --git a/src/main/include/photon/serde/PnpResultSerde.h b/src/main/include/photon/serde/PnpResultSerde.h new file mode 100644 index 0000000..8e507b0 --- /dev/null +++ b/src/main/include/photon/serde/PnpResultSerde.h @@ -0,0 +1,52 @@ +/* + * 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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +#include + +// Include myself +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/PnpResult.h" + +// Includes for dependant types +#include +#include + +namespace photon { + +template <> +struct WPILIB_DLLEXPORT SerdeType { + static constexpr std::string_view GetSchemaHash() { + return "0d1f2546b00f24718e30f38d206d4491"; + } + + static constexpr std::string_view GetSchema() { + return "Transform3d best;Transform3d alt;float64 bestReprojErr;float64 " + "altReprojErr;float64 ambiguity;"; + } + + static photon::PnpResult Unpack(photon::Packet& packet); + static void Pack(photon::Packet& packet, const photon::PnpResult& value); +}; + +static_assert(photon::PhotonStructSerializable); + +} // namespace photon diff --git a/src/main/include/photon/serde/TargetCornerSerde.h b/src/main/include/photon/serde/TargetCornerSerde.h new file mode 100644 index 0000000..27ce589 --- /dev/null +++ b/src/main/include/photon/serde/TargetCornerSerde.h @@ -0,0 +1,50 @@ +/* + * 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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +#include + +// Include myself +#include "photon/dataflow/structures/Packet.h" +#include "photon/targeting/TargetCorner.h" + +// Includes for dependant types +#include + +namespace photon { + +template <> +struct WPILIB_DLLEXPORT SerdeType { + static constexpr std::string_view GetSchemaHash() { + return "22b1ff7551d10215af6fb3672fe4eda8"; + } + + static constexpr std::string_view GetSchema() { + return "float64 x;float64 y;"; + } + + static photon::TargetCorner Unpack(photon::Packet& packet); + static void Pack(photon::Packet& packet, const photon::TargetCorner& value); +}; + +static_assert(photon::PhotonStructSerializable); + +} // namespace photon diff --git a/src/main/include/photon/simulation/PhotonCameraSim.h b/src/main/include/photon/simulation/PhotonCameraSim.h index fe2082b..ea6591c 100644 --- a/src/main/include/photon/simulation/PhotonCameraSim.h +++ b/src/main/include/photon/simulation/PhotonCameraSim.h @@ -91,7 +91,7 @@ class PhotonCameraSim { void SubmitProcessedFrame(const PhotonPipelineResult& result); void SubmitProcessedFrame(const PhotonPipelineResult& result, - uint64_t recieveTimestamp); + uint64_t ReceiveTimestamp); SimCameraProperties prop; diff --git a/src/main/include/photon/struct/MultiTargetPNPResultStruct.h b/src/main/include/photon/struct/MultiTargetPNPResultStruct.h new file mode 100644 index 0000000..a3c691f --- /dev/null +++ b/src/main/include/photon/struct/MultiTargetPNPResultStruct.h @@ -0,0 +1,40 @@ +/* + * 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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +// Includes for dependant types +#include + +#include + +#include "photon/targeting/PnpResult.h" + +namespace photon { + +struct MultiTargetPNPResult_PhotonStruct { + photon::PnpResult estimatedPose; + std::vector fiducialIDsUsed; + + friend bool operator==(MultiTargetPNPResult_PhotonStruct const&, + MultiTargetPNPResult_PhotonStruct const&) = default; +}; + +} // namespace photon diff --git a/src/main/include/photon/struct/PhotonPipelineMetadataStruct.h b/src/main/include/photon/struct/PhotonPipelineMetadataStruct.h new file mode 100644 index 0000000..a6e3440 --- /dev/null +++ b/src/main/include/photon/struct/PhotonPipelineMetadataStruct.h @@ -0,0 +1,37 @@ +/* + * 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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +// Includes for dependant types +#include + +namespace photon { + +struct PhotonPipelineMetadata_PhotonStruct { + int64_t sequenceID; + int64_t captureTimestampMicros; + int64_t publishTimestampMicros; + + friend bool operator==(PhotonPipelineMetadata_PhotonStruct const&, + PhotonPipelineMetadata_PhotonStruct const&) = default; +}; + +} // namespace photon diff --git a/src/main/include/photon/struct/PhotonPipelineResultStruct.h b/src/main/include/photon/struct/PhotonPipelineResultStruct.h new file mode 100644 index 0000000..a6f65d9 --- /dev/null +++ b/src/main/include/photon/struct/PhotonPipelineResultStruct.h @@ -0,0 +1,44 @@ +/* + * 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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +// Includes for dependant types +#include + +#include +#include + +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PhotonPipelineMetadata.h" +#include "photon/targeting/PhotonTrackedTarget.h" + +namespace photon { + +struct PhotonPipelineResult_PhotonStruct { + photon::PhotonPipelineMetadata metadata; + std::vector targets; + std::optional multitagResult; + + friend bool operator==(PhotonPipelineResult_PhotonStruct const&, + PhotonPipelineResult_PhotonStruct const&) = default; +}; + +} // namespace photon diff --git a/src/main/include/photon/struct/PhotonTrackedTargetStruct.h b/src/main/include/photon/struct/PhotonTrackedTargetStruct.h new file mode 100644 index 0000000..b6f480f --- /dev/null +++ b/src/main/include/photon/struct/PhotonTrackedTargetStruct.h @@ -0,0 +1,52 @@ +/* + * 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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +// Includes for dependant types +#include + +#include + +#include + +#include "photon/targeting/TargetCorner.h" + +namespace photon { + +struct PhotonTrackedTarget_PhotonStruct { + double yaw; + double pitch; + double area; + double skew; + int32_t fiducialId; + int32_t objDetectId; + float objDetectConf; + frc::Transform3d bestCameraToTarget; + frc::Transform3d altCameraToTarget; + double poseAmbiguity; + std::vector minAreaRectCorners; + std::vector detectedCorners; + + friend bool operator==(PhotonTrackedTarget_PhotonStruct const&, + PhotonTrackedTarget_PhotonStruct const&) = default; +}; + +} // namespace photon diff --git a/src/main/include/photon/struct/PnpResultStruct.h b/src/main/include/photon/struct/PnpResultStruct.h new file mode 100644 index 0000000..5afa65f --- /dev/null +++ b/src/main/include/photon/struct/PnpResultStruct.h @@ -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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +// Includes for dependant types +#include + +#include + +namespace photon { + +struct PnpResult_PhotonStruct { + frc::Transform3d best; + frc::Transform3d alt; + double bestReprojErr; + double altReprojErr; + double ambiguity; + + friend bool operator==(PnpResult_PhotonStruct const&, + PnpResult_PhotonStruct const&) = default; +}; + +} // namespace photon diff --git a/src/main/include/photon/struct/TargetCornerStruct.h b/src/main/include/photon/struct/TargetCornerStruct.h new file mode 100644 index 0000000..2cb9b50 --- /dev/null +++ b/src/main/include/photon/struct/TargetCornerStruct.h @@ -0,0 +1,36 @@ +/* + * 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 . + */ + +#pragma once + +// THIS std::FILE WAS AUTO-GENERATED BY ./photon-serde/generate_messages.py. DO +// NOT MODIFY + +// Includes for dependant types +#include + +namespace photon { + +struct TargetCorner_PhotonStruct { + double x; + double y; + + friend bool operator==(TargetCorner_PhotonStruct const&, + TargetCorner_PhotonStruct const&) = default; +}; + +} // namespace photon diff --git a/src/main/include/photon/targeting/MultiTargetPNPResult.h b/src/main/include/photon/targeting/MultiTargetPNPResult.h index 76a323e..3c8790e 100644 --- a/src/main/include/photon/targeting/MultiTargetPNPResult.h +++ b/src/main/include/photon/targeting/MultiTargetPNPResult.h @@ -17,21 +17,29 @@ #pragma once +#include + #include #include -#include "PNPResult.h" +#include "PnpResult.h" #include "photon/dataflow/structures/Packet.h" +#include "photon/struct/MultiTargetPNPResultStruct.h" namespace photon { -class MultiTargetPNPResult { +class MultiTargetPNPResult : public MultiTargetPNPResult_PhotonStruct { + using Base = MultiTargetPNPResult_PhotonStruct; + public: - PNPResult result; - wpi::SmallVector fiducialIdsUsed; + explicit MultiTargetPNPResult(Base&& data) : Base(data) {} - bool operator==(const MultiTargetPNPResult& other) const; + template + explicit MultiTargetPNPResult(Args&&... args) + : Base{std::forward(args)...} {} - friend Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result); - friend Packet& operator>>(Packet& packet, MultiTargetPNPResult& result); + friend bool operator==(MultiTargetPNPResult const&, + MultiTargetPNPResult const&) = default; }; } // namespace photon + +#include "photon/serde/MultiTargetPNPResultSerde.h" diff --git a/src/main/include/photon/targeting/PNPResult.h b/src/main/include/photon/targeting/PNPResult.h index 49f73ed..f07b46c 100644 --- a/src/main/include/photon/targeting/PNPResult.h +++ b/src/main/include/photon/targeting/PNPResult.h @@ -17,29 +17,26 @@ #pragma once +#include + #include #include "photon/dataflow/structures/Packet.h" +#include "photon/struct/PnpResultStruct.h" namespace photon { -class PNPResult { - public: - // This could be wrapped in an std::optional, but chose to do it this way to - // mirror Java - bool isPresent{false}; - - frc::Transform3d best{}; - double bestReprojErr{0}; - - frc::Transform3d alt{}; - double altReprojErr{0}; +struct PnpResult : public PnpResult_PhotonStruct { + using Base = PnpResult_PhotonStruct; - double ambiguity{0}; + explicit PnpResult(Base&& data) : Base(data) {} - bool operator==(const PNPResult& other) const; + template + explicit PnpResult(Args&&... args) : Base{std::forward(args)...} {} - friend Packet& operator<<(Packet& packet, const PNPResult& target); - friend Packet& operator>>(Packet& packet, PNPResult& target); + friend bool operator==(PnpResult const&, PnpResult const&) = default; }; + } // namespace photon + +#include "photon/serde/PnpResultSerde.h" diff --git a/src/main/include/photon/targeting/PhotonPipelineMetadata.h b/src/main/include/photon/targeting/PhotonPipelineMetadata.h new file mode 100644 index 0000000..ba72256 --- /dev/null +++ b/src/main/include/photon/targeting/PhotonPipelineMetadata.h @@ -0,0 +1,40 @@ +/* + * 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 . + */ + +#pragma once + +#include + +#include "photon/struct/PhotonPipelineMetadataStruct.h" + +namespace photon { +class PhotonPipelineMetadata : public PhotonPipelineMetadata_PhotonStruct { + using Base = PhotonPipelineMetadata_PhotonStruct; + + public: + explicit PhotonPipelineMetadata(Base&& data) : Base(data) {} + + template + explicit PhotonPipelineMetadata(Args&&... args) + : Base{std::forward(args)...} {} + + friend bool operator==(PhotonPipelineMetadata const&, + PhotonPipelineMetadata const&) = default; +}; +} // namespace photon + +#include "photon/serde/PhotonPipelineMetadataSerde.h" diff --git a/src/main/include/photon/targeting/PhotonPipelineResult.h b/src/main/include/photon/targeting/PhotonPipelineResult.h index a34d1df..2b97384 100644 --- a/src/main/include/photon/targeting/PhotonPipelineResult.h +++ b/src/main/include/photon/targeting/PhotonPipelineResult.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -27,34 +28,41 @@ #include "MultiTargetPNPResult.h" #include "PhotonTrackedTarget.h" #include "photon/dataflow/structures/Packet.h" +#include "photon/struct/PhotonPipelineResultStruct.h" namespace photon { /** * Represents a pipeline result from a PhotonCamera. */ -class PhotonPipelineResult { +class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { + using Base = PhotonPipelineResult_PhotonStruct; + public: - /** - * Constructs an empty pipeline result - */ - PhotonPipelineResult() = default; + PhotonPipelineResult() : Base() {} + explicit PhotonPipelineResult(Base&& data) : Base(data) {} + + // Don't forget to deal with our ntRecieveTimestamp + PhotonPipelineResult(const PhotonPipelineResult& other) + : Base(other), ntReceiveTimestamp(other.ntReceiveTimestamp) {} + PhotonPipelineResult(PhotonPipelineResult& other) + : Base(other), ntReceiveTimestamp(other.ntReceiveTimestamp) {} + PhotonPipelineResult(PhotonPipelineResult&& other) + : Base(std::move(other)), + ntReceiveTimestamp(std::move(other.ntReceiveTimestamp)) {} + auto& operator=(const PhotonPipelineResult& other) { + Base::operator=(other); + ntReceiveTimestamp = other.ntReceiveTimestamp; + return *this; + } + auto& operator=(PhotonPipelineResult&& other) { + ntReceiveTimestamp = other.ntReceiveTimestamp; + Base::operator=(std::move(other)); + return *this; + } - /** - * Constructs a pipeline result. - * @param sequenceID The number of frames processed by this camera since boot - * @param captureTimestamp The time, in uS in the coprocessor's timebase, that - * the coprocessor captured the image this result contains the targeting info - * of - * @param publishTimestamp The time, in uS in the coprocessor's timebase, that - * the coprocessor published targeting info - * @param targets The list of targets identified by the pipeline. - * @param multitagResult The multitarget result. Default to empty - */ - PhotonPipelineResult(int64_t sequenceID, - units::microsecond_t captureTimestamp, - units::microsecond_t publishTimestamp, - std::span targets, - MultiTargetPNPResult multitagResult = {}); + template + explicit PhotonPipelineResult(Args&&... args) + : Base{std::forward(args)...} {} /** * Returns the best target in this pipeline result. If there are no targets, @@ -73,7 +81,7 @@ class PhotonPipelineResult { "http://docs.photonvision.org"); HAS_WARNED = true; } - return HasTargets() ? targets[0] : PhotonTrackedTarget(); + return HasTargets() ? targets[0] : PhotonTrackedTarget{}; } /** @@ -81,7 +89,8 @@ class PhotonPipelineResult { * @return The latency in the pipeline. */ units::millisecond_t GetLatency() const { - return publishTimestamp - captureTimestamp; + return units::microsecond_t{static_cast( + metadata.publishTimestampMicros - metadata.captureTimestampMicros)}; } /** @@ -91,7 +100,7 @@ class PhotonPipelineResult { * with a timestamp. */ units::second_t GetTimestamp() const { - return ntRecieveTimestamp - (publishTimestamp - captureTimestamp); + return ntReceiveTimestamp - GetLatency(); } /** @@ -99,17 +108,19 @@ class PhotonPipelineResult { * Be sure to check getMultiTagResult().estimatedPose.isPresent before using * the pose estimate! */ - const MultiTargetPNPResult& MultiTagResult() const { return multitagResult; } + const std::optional& MultiTagResult() const { + return multitagResult; + } /** * The number of non-empty frames processed by this camera since boot. Useful * to checking if a camera is alive. */ - int64_t SequenceID() const { return sequenceID; } + int64_t SequenceID() const { return metadata.sequenceID; } - /** Sets the FPGA timestamp this result was recieved by robot code */ - void SetRecieveTimestamp(const units::second_t timestamp) { - this->ntRecieveTimestamp = timestamp; + /** Sets the FPGA timestamp this result was Received by robot code */ + void SetReceiveTimestamp(const units::second_t timestamp) { + this->ntReceiveTimestamp = timestamp; } /** @@ -120,30 +131,22 @@ class PhotonPipelineResult { /** * Returns a reference to the vector of targets. + *

Returned in the order set by target sort mode.

* @return A reference to the vector of targets. */ const std::span GetTargets() const { return targets; } - bool operator==(const PhotonPipelineResult& other) const; - - friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result); - friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result); + friend bool operator==(PhotonPipelineResult const&, + PhotonPipelineResult const&) = default; - // Mirror of the heartbeat entry -- monotonically increasing - int64_t sequenceID = -1; - - // Image capture and NT publish timestamp, in microseconds and in the - // coprocessor timebase. As reported by WPIUtilJNI::now. - units::microsecond_t captureTimestamp; - units::microsecond_t publishTimestamp; // Since we don't trust NT time sync, keep track of when we got this packet // into robot code - units::microsecond_t ntRecieveTimestamp = -1_s; + units::microsecond_t ntReceiveTimestamp = -1_s; - wpi::SmallVector targets; - MultiTargetPNPResult multitagResult; inline static bool HAS_WARNED = false; }; } // namespace photon + +#include "photon/serde/PhotonPipelineResultSerde.h" diff --git a/src/main/include/photon/targeting/PhotonTrackedTarget.h b/src/main/include/photon/targeting/PhotonTrackedTarget.h index bbd6721..66f6a06 100644 --- a/src/main/include/photon/targeting/PhotonTrackedTarget.h +++ b/src/main/include/photon/targeting/PhotonTrackedTarget.h @@ -25,36 +25,23 @@ #include #include -#include "photon/dataflow/structures/Packet.h" +#include "photon/struct/PhotonTrackedTargetStruct.h" namespace photon { /** * Represents a tracked target within a pipeline. */ -class PhotonTrackedTarget { +class PhotonTrackedTarget : public PhotonTrackedTarget_PhotonStruct { + using Base = PhotonTrackedTarget_PhotonStruct; + public: - /** - * Constructs an empty target. - */ PhotonTrackedTarget() = default; - /** - * Constructs a target. - * @param yaw The yaw of the target. - * @param pitch The pitch of the target. - * @param area The area of the target. - * @param skew The skew of the target. - * @param pose The camera-relative pose of the target. - * @param alternatePose The alternate camera-relative pose of the target. - * @param minAreaRectCorners The corners of the bounding rectangle. - * @param detectedCorners All detected corners - */ - PhotonTrackedTarget( - double yaw, double pitch, double area, double skew, int fiducialID, - int objDetectCassId, float objDetectConf, const frc::Transform3d& pose, - const frc::Transform3d& alternatePose, double ambiguity, - const wpi::SmallVector, 4> minAreaRectCorners, - const std::vector> detectedCorners); + explicit PhotonTrackedTarget(Base&& data) : Base(data) {} + + template + explicit PhotonTrackedTarget(Args&&... args) + : Base{std::forward(args)...} {} /** * Returns the target yaw (positive-left). @@ -103,8 +90,7 @@ class PhotonTrackedTarget { * down), in no particular order, of the minimum area bounding rectangle of * this target */ - const wpi::SmallVector, 4>& GetMinAreaRectCorners() - const { + const std::vector& GetMinAreaRectCorners() const { return minAreaRectCorners; } @@ -119,7 +105,7 @@ class PhotonTrackedTarget { * V + Y | | * 0 ----- 1 */ - const std::vector>& GetDetectedCorners() const { + const std::vector& GetDetectedCorners() const { return detectedCorners; } @@ -149,22 +135,9 @@ class PhotonTrackedTarget { return altCameraToTarget; } - bool operator==(const PhotonTrackedTarget& other) const; - - friend Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target); - friend Packet& operator>>(Packet& packet, PhotonTrackedTarget& target); - - double yaw = 0; - double pitch = 0; - double area = 0; - double skew = 0; - int fiducialId; - int objDetectId; - float objDetectConf; - frc::Transform3d bestCameraToTarget; - frc::Transform3d altCameraToTarget; - double poseAmbiguity; - wpi::SmallVector, 4> minAreaRectCorners; - std::vector> detectedCorners; + friend bool operator==(PhotonTrackedTarget const&, + PhotonTrackedTarget const&) = default; }; } // namespace photon + +#include "photon/serde/PhotonTrackedTargetSerde.h" diff --git a/src/main/include/photon/targeting/TargetCorner.h b/src/main/include/photon/targeting/TargetCorner.h new file mode 100644 index 0000000..48d928a --- /dev/null +++ b/src/main/include/photon/targeting/TargetCorner.h @@ -0,0 +1,38 @@ +/* + * 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 . + */ + +#include + +#include "photon/struct/TargetCornerStruct.h" + +#pragma once + +namespace photon { +class TargetCorner : public TargetCorner_PhotonStruct { + using Base = TargetCorner_PhotonStruct; + + public: + explicit TargetCorner(Base&& data) : Base(data) {} + + template + explicit TargetCorner(Args&&... args) : Base{std::forward(args)...} {} + + friend bool operator==(TargetCorner const&, TargetCorner const&) = default; +}; +} // namespace photon + +#include "photon/serde/TargetCornerSerde.h" diff --git a/src/main/include/str/Vision.h b/src/main/include/str/Vision.h index b65b3e6..25bdf1d 100644 --- a/src/main/include/str/Vision.h +++ b/src/main/include/str/Vision.h @@ -24,7 +24,7 @@ class Vision { std::array cameras{ Camera{consts::vision::FL_CAM_NAME, consts::vision::FL_ROBOT_TO_CAM, consts::vision::SINGLE_TAG_STD_DEV, - consts::vision::MULTI_TAG_STD_DEV, false}, + consts::vision::MULTI_TAG_STD_DEV, true}, Camera{consts::vision::FR_CAM_NAME, consts::vision::FR_ROBOT_TO_CAM, consts::vision::SINGLE_TAG_STD_DEV, consts::vision::MULTI_TAG_STD_DEV, false},