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

Send corners of min area rectangles (#382)

Browse Source 
Adds a new corners entry to targets. Breaks byte-packed backwards compatibility. 

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2022-01-10 20:31:36 -08:00
committed by GitHub
parent e6d8e05b91
commit 3ad476bc28
12 changed files with 215 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp
View File

@@ -18,19 +18,28 @@
#include "photonlib/PhotonTrackedTarget.h"
#include <iostream>
#include <utility>
#include <frc/geometry/Translation2d.h>
#include <wpi/SmallVector.h>
namespace photonlib {
PhotonTrackedTarget::PhotonTrackedTarget(double yaw, double pitch, double area,
double skew,
const frc::Transform2d& pose)
: yaw(yaw), pitch(pitch), area(area), skew(skew), cameraToTarget(pose) {}
PhotonTrackedTarget::PhotonTrackedTarget(
double yaw, double pitch, double area, double skew,
const frc::Transform2d& pose,
const wpi::SmallVector<std::pair<double, double>, 4> corners)
: yaw(yaw),
pitch(pitch),
area(area),
skew(skew),
cameraToTarget(pose),
corners(corners) {}
bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
return other.yaw == yaw && other.pitch == pitch && other.area == area &&
other.skew == skew && other.cameraToTarget == cameraToTarget;
other.skew == skew && other.cameraToTarget == cameraToTarget &&
other.corners == corners;
}
bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
@@ -38,10 +47,16 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const {
}
Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
return packet << target.yaw << target.pitch << target.area << target.skew
<< target.cameraToTarget.Translation().X().value()
<< target.cameraToTarget.Translation().Y().value()
<< target.cameraToTarget.Rotation().Degrees().value();
packet << target.yaw << target.pitch << target.area << target.skew
<< target.cameraToTarget.Translation().X().value()
<< target.cameraToTarget.Translation().Y().value()
<< target.cameraToTarget.Rotation().Degrees().value();
for (int i = 0; i < 4; i++) {
packet << target.corners[i].first << target.corners[i].second;
}
return packet;
}
Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
@@ -54,6 +69,15 @@ Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
target.cameraToTarget =
frc::Transform2d(frc::Translation2d(units::meter_t(x), units::meter_t(y)),
units::degree_t(rot));
target.corners.clear();
for (int i = 0; i < 4; i++) {
double first = 0;
double second = 0;
packet >> first >> second;
target.corners.emplace_back(first, second);
}
return packet;
}