mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-28 02:11:40 +00:00
[photon-targeting] Move C++ targeting classes to photon-targetting (#1009)
* add classes to targeting and update gradle * rename me * Finish cleanup * Formatting fixes * just use common.gradle * Update build.gradle * Update config.gradle * remove typo * simplify * Add Packet Headers * move simulation classes into simulation folder * draw in dependency * fix * Everything working minus tests cause im lazy * formatting fixes REMEMBER TO REMOVE UNUSED IMPORTS, IM JUST TOO LAZY TO CHECK RN * move packet test to targeting * Formatting fixes * remove TargetCorner from c++ im not 100% sure but just doing std::pair<double, double> is sufficient and shouldnt conflict with protobuf * think i added packet * fix namespace issue * organize imports in photon-targeting * Formatting fixes * remove ctors * fix typo * Add PNP and Multitag packet tests * revert TargetCorner class * Add Test placeholders * remove annoying print * Reorganize build and publish process channeling inner Thad * add targeting as flag * Update config.gradle * fix issue with platform binaries not building * Update photonlib.json.in casing still needs to be checked * add minimum level back * add back UTF-8 encoding of javadoc * simplify publish model for photon-lib * fix windows symbol generation * formatting fixes * move task from main gradle to config * Update config.gradle
This commit is contained in:
Sriman Achanta
@@ -0,0 +1,137 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <networktables/NetworkTableInstance.h>
|
||||
|
||||
#include "photon/PhotonCamera.h"
|
||||
#include "photon/PhotonTargetSortMode.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"
|
||||
|
||||
namespace photon {
|
||||
class SimPhotonCamera : public PhotonCamera {
|
||||
public:
|
||||
SimPhotonCamera(nt::NetworkTableInstance instance,
|
||||
const std::string& cameraName)
|
||||
: PhotonCamera(instance, cameraName) {
|
||||
latencyMillisEntry = rootTable->GetEntry("latencyMillis");
|
||||
hasTargetEntry = rootTable->GetEntry("hasTargetEntry");
|
||||
targetPitchEntry = rootTable->GetEntry("targetPitchEntry");
|
||||
targetYawEntry = rootTable->GetEntry("targetYawEntry");
|
||||
targetAreaEntry = rootTable->GetEntry("targetAreaEntry");
|
||||
targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
|
||||
targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
|
||||
rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("rawBytes");
|
||||
versionEntry = instance.GetTable("photonvision")->GetEntry("version");
|
||||
// versionEntry.SetString(PhotonVersion.versionString);
|
||||
}
|
||||
|
||||
explicit SimPhotonCamera(const std::string& cameraName)
|
||||
: SimPhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
|
||||
|
||||
virtual ~SimPhotonCamera() = default;
|
||||
|
||||
/**
|
||||
* Simulate one processed frame of vision data, putting one result to NT.
|
||||
*
|
||||
* @param latency Latency of the provided frame
|
||||
* @param targetList List of targets detected
|
||||
*/
|
||||
void SubmitProcessedFrame(units::millisecond_t latency,
|
||||
std::vector<PhotonTrackedTarget> targetList) {
|
||||
SubmitProcessedFrame(latency, PhotonTargetSortMode::LeftMost(), targetList);
|
||||
}
|
||||
|
||||
/**
|
||||
* Simulate one processed frame of vision data, putting one result to NT.
|
||||
*
|
||||
* @param latency Latency of the provided frame
|
||||
* @param sortMode Order in which to sort targets
|
||||
* @param targetList List of targets detected
|
||||
*/
|
||||
void SubmitProcessedFrame(
|
||||
units::millisecond_t latency,
|
||||
std::function<bool(const PhotonTrackedTarget& target1,
|
||||
const PhotonTrackedTarget& target2)>
|
||||
sortMode,
|
||||
std::vector<PhotonTrackedTarget> targetList) {
|
||||
latencyMillisEntry.SetDouble(latency.to<double>());
|
||||
std::sort(targetList.begin(), targetList.end(),
|
||||
[&](auto lhs, auto rhs) { return sortMode(lhs, rhs); });
|
||||
PhotonPipelineResult newResult{latency, targetList};
|
||||
Packet packet{};
|
||||
packet << newResult;
|
||||
|
||||
rawBytesPublisher.Set(
|
||||
std::span{packet.GetData().data(), packet.GetDataSize()});
|
||||
|
||||
bool hasTargets = newResult.HasTargets();
|
||||
hasTargetEntry.SetBoolean(hasTargets);
|
||||
if (!hasTargets) {
|
||||
targetPitchEntry.SetDouble(0.0);
|
||||
targetYawEntry.SetDouble(0.0);
|
||||
targetAreaEntry.SetDouble(0.0);
|
||||
targetPoseEntry.SetDoubleArray(
|
||||
std::vector<double>{0.0, 0.0, 0.0, 0, 0, 0, 0});
|
||||
targetSkewEntry.SetDouble(0.0);
|
||||
} else {
|
||||
PhotonTrackedTarget bestTarget = newResult.GetBestTarget();
|
||||
targetPitchEntry.SetDouble(bestTarget.GetPitch());
|
||||
targetYawEntry.SetDouble(bestTarget.GetYaw());
|
||||
targetAreaEntry.SetDouble(bestTarget.GetArea());
|
||||
targetSkewEntry.SetDouble(bestTarget.GetSkew());
|
||||
|
||||
frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
|
||||
targetPoseEntry.SetDoubleArray(std::vector<double>{
|
||||
transform.X().to<double>(), transform.Y().to<double>(),
|
||||
transform.Z().to<double>(), transform.Rotation().GetQuaternion().W(),
|
||||
transform.Rotation().GetQuaternion().X(),
|
||||
transform.Rotation().GetQuaternion().Y(),
|
||||
transform.Rotation().GetQuaternion().Z()});
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
nt::NetworkTableEntry latencyMillisEntry;
|
||||
nt::NetworkTableEntry hasTargetEntry;
|
||||
nt::NetworkTableEntry targetPitchEntry;
|
||||
nt::NetworkTableEntry targetYawEntry;
|
||||
nt::NetworkTableEntry targetAreaEntry;
|
||||
nt::NetworkTableEntry targetSkewEntry;
|
||||
nt::NetworkTableEntry targetPoseEntry;
|
||||
nt::NetworkTableEntry versionEntry;
|
||||
nt::RawPublisher rawBytesPublisher;
|
||||
};
|
||||
} // namespace photon
|
||||
@@ -0,0 +1,226 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <frc/smartdashboard/Field2d.h>
|
||||
#include <frc/smartdashboard/SmartDashboard.h>
|
||||
#include <units/angle.h>
|
||||
#include <units/area.h>
|
||||
|
||||
#include "SimPhotonCamera.h"
|
||||
#include "SimVisionTarget.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"
|
||||
|
||||
namespace photon {
|
||||
class SimVisionSystem {
|
||||
public:
|
||||
SimPhotonCamera cam;
|
||||
units::radian_t camHorizFOV{0};
|
||||
units::radian_t camVertFOV{0};
|
||||
units::meter_t maxLEDRange{0};
|
||||
int cameraResWidth{0};
|
||||
int cameraResHeight{0};
|
||||
double minTargetArea{0.0};
|
||||
frc::Transform3d cameraToRobot;
|
||||
|
||||
frc::Field2d dbgField;
|
||||
frc::FieldObject2d* dbgRobot;
|
||||
frc::FieldObject2d* dbgCamera;
|
||||
|
||||
std::vector<SimVisionTarget> targetList;
|
||||
|
||||
/**
|
||||
* Create a simulated vision system involving a camera and coprocessor mounted
|
||||
* on a mobile robot running PhotonVision, detecting one or more targets
|
||||
* scattered around the field. This assumes a fairly simple and
|
||||
* distortion-less pinhole camera model.
|
||||
*
|
||||
* @param camName Name of the PhotonVision camera to create. Align it with the
|
||||
* settings you use in the PhotonVision GUI.
|
||||
* @param camDiagFOV Diagonal Field of View of the camera used. Align it with
|
||||
* the manufacturer specifications, and/or whatever is configured in the
|
||||
* PhotonVision Setting page.
|
||||
* @param cameraToRobot Transform to move from the camera's mount position to
|
||||
* the robot's position
|
||||
* @param maxLEDRange Maximum distance at which your camera can illuminate the
|
||||
* target and make it visible. Set to 9000 or more if your vision system does
|
||||
* not rely on LED's.
|
||||
* @param cameraResWidth Width of your camera's image sensor in pixels
|
||||
* @param cameraResHeight Height of your camera's image sensor in pixels
|
||||
* @param minTargetArea Minimum area that that the target should be before
|
||||
* it's recognized as a target by the camera. Match this with your contour
|
||||
* filtering settings in the PhotonVision GUI.
|
||||
*/
|
||||
SimVisionSystem(std::string camName, units::degree_t camDiagFOV,
|
||||
frc::Transform3d cameraToRobot, units::meter_t maxLEDRange,
|
||||
int cameraResWidth, int cameraResHeight, double minTargetArea)
|
||||
: cam(camName),
|
||||
camHorizFOV((camDiagFOV * cameraResWidth) /
|
||||
std::hypot(cameraResWidth, cameraResHeight)),
|
||||
camVertFOV((camDiagFOV * cameraResHeight) /
|
||||
std::hypot(cameraResWidth, cameraResHeight)),
|
||||
maxLEDRange(maxLEDRange),
|
||||
cameraResWidth(cameraResWidth),
|
||||
cameraResHeight(cameraResHeight),
|
||||
minTargetArea(minTargetArea),
|
||||
cameraToRobot(cameraToRobot),
|
||||
dbgField(),
|
||||
dbgRobot(dbgField.GetRobotObject()),
|
||||
dbgCamera(dbgField.GetObject(camName + " Camera")) {
|
||||
frc::SmartDashboard::PutData(camName + " Sim Field", &dbgField);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a target on the field which your vision system is designed to detect.
|
||||
* The PhotonCamera from this system will report the location of the robot
|
||||
* relative to the subset of these targets which are visible from the given
|
||||
* robot position.
|
||||
*
|
||||
* @param target Target to add to the simulated field
|
||||
*/
|
||||
void AddSimVisionTarget(SimVisionTarget target) {
|
||||
targetList.push_back(target);
|
||||
dbgField.GetObject("Target " + std::to_string(target.targetId))
|
||||
->SetPose(target.targetPose.ToPose2d());
|
||||
}
|
||||
|
||||
/**
|
||||
* Clears all sim vision targets.
|
||||
* This is useful for switching alliances and needing to repopulate the sim
|
||||
* targets. NOTE: Old targets will still show on the Field2d unless
|
||||
* overwritten by new targets with the same ID
|
||||
*/
|
||||
void ClearVisionTargets() { targetList.clear(); }
|
||||
|
||||
/**
|
||||
* Adjust the camera position relative to the robot. Use this if your camera
|
||||
* is on a gimbal or turret or some other mobile platform.
|
||||
*
|
||||
* @param newCameraToRobot New Transform from the robot to the camera
|
||||
*/
|
||||
void MoveCamera(frc::Transform3d newCameraToRobot) {
|
||||
cameraToRobot = newCameraToRobot;
|
||||
}
|
||||
|
||||
/**
|
||||
* Periodic update. Call this once per frame of image data you wish to process
|
||||
* and send to NetworkTables
|
||||
*
|
||||
* @param robotPose current pose of the robot on the field. Will be used to
|
||||
* calculate which targets are actually in view, where they are at relative to
|
||||
* the robot, and relevant PhotonVision parameters.
|
||||
*/
|
||||
void ProcessFrame(frc::Pose2d robotPose) {
|
||||
ProcessFrame(frc::Pose3d{
|
||||
robotPose.X(), robotPose.Y(), 0.0_m,
|
||||
frc::Rotation3d{0_rad, 0_rad, robotPose.Rotation().Radians()}});
|
||||
}
|
||||
|
||||
/**
|
||||
* Periodic update. Call this once per frame of image data you wish to process
|
||||
* and send to NetworkTables
|
||||
*
|
||||
* @param robotPose current pose of the robot in space. Will be used to
|
||||
* calculate which targets are actually in view, where they are at relative to
|
||||
* the robot, and relevant PhotonVision parameters.
|
||||
*/
|
||||
void ProcessFrame(frc::Pose3d robotPose) {
|
||||
frc::Pose3d cameraPose = robotPose.TransformBy(cameraToRobot.Inverse());
|
||||
dbgRobot->SetPose(robotPose.ToPose2d());
|
||||
dbgCamera->SetPose(cameraPose.ToPose2d());
|
||||
|
||||
std::vector<PhotonTrackedTarget> visibleTargetList{};
|
||||
|
||||
for (const auto& target : targetList) {
|
||||
frc::Transform3d camToTargetTransform{cameraPose, target.targetPose};
|
||||
frc::Translation3d camToTargetTranslation{
|
||||
camToTargetTransform.Translation()};
|
||||
|
||||
frc::Translation3d altTranslation{camToTargetTranslation.X(),
|
||||
-1.0 * camToTargetTranslation.Y(),
|
||||
camToTargetTranslation.Z()};
|
||||
frc::Rotation3d altRotation{camToTargetTransform.Rotation() * -1.0};
|
||||
frc::Transform3d camToTargetAltTransform{altTranslation, altRotation};
|
||||
units::meter_t dist{camToTargetTranslation.Norm()};
|
||||
double areaPixels{target.targetArea / GetM2PerPx(dist)};
|
||||
units::radian_t yaw{units::math::atan2(camToTargetTranslation.Y(),
|
||||
camToTargetTranslation.X())};
|
||||
units::meter_t cameraHeightOffGround{cameraPose.Z()};
|
||||
units::meter_t targetHeightAboveGround(target.targetPose.Z());
|
||||
units::radian_t camPitch{cameraPose.Rotation().Y()};
|
||||
frc::Transform2d transformAlongGround{cameraPose.ToPose2d(),
|
||||
target.targetPose.ToPose2d()};
|
||||
units::meter_t distanceAlongGround{
|
||||
transformAlongGround.Translation().Norm()};
|
||||
units::radian_t pitch =
|
||||
units::math::atan2(targetHeightAboveGround - cameraHeightOffGround,
|
||||
distanceAlongGround) -
|
||||
camPitch;
|
||||
|
||||
if (CamCamSeeTarget(dist, yaw, pitch, areaPixels)) {
|
||||
visibleTargetList.push_back(
|
||||
PhotonTrackedTarget{yaw.convert<units::degree>().to<double>(),
|
||||
pitch.convert<units::degree>().to<double>(),
|
||||
areaPixels,
|
||||
0.0,
|
||||
target.targetId,
|
||||
camToTargetTransform,
|
||||
// TODO sim alternate pose
|
||||
camToTargetTransform,
|
||||
// TODO ambiguity
|
||||
0.0,
|
||||
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
|
||||
{{0, 0}, {0, 0}, {0, 0}, {0, 0}}});
|
||||
}
|
||||
|
||||
cam.SubmitProcessedFrame(0_s, visibleTargetList);
|
||||
}
|
||||
}
|
||||
|
||||
units::square_meter_t GetM2PerPx(units::meter_t dist) {
|
||||
units::meter_t widthMPerPx =
|
||||
2 * dist * units::math::tan(camHorizFOV / 2) / cameraResWidth;
|
||||
units::meter_t heightMPerPx =
|
||||
2 * dist * units::math::tan(camVertFOV / 2) / cameraResHeight;
|
||||
return widthMPerPx * heightMPerPx;
|
||||
}
|
||||
|
||||
bool CamCamSeeTarget(units::meter_t dist, units::radian_t yaw,
|
||||
units::radian_t pitch, double area) {
|
||||
bool inRange = dist < maxLEDRange;
|
||||
bool inHorizAngle = units::math::abs(yaw) < camHorizFOV / 2;
|
||||
bool inVertAngle = units::math::abs(pitch) < camVertFOV / 2;
|
||||
bool targetBigEnough = area > minTargetArea;
|
||||
return (inRange && inHorizAngle && inVertAngle && targetBigEnough);
|
||||
}
|
||||
};
|
||||
} // namespace photon
|
||||
@@ -0,0 +1,65 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) PhotonVision
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <frc/geometry/Pose3d.h>
|
||||
#include <units/area.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"
|
||||
|
||||
namespace photon {
|
||||
class SimVisionTarget {
|
||||
public:
|
||||
SimVisionTarget() = default;
|
||||
|
||||
/**
|
||||
* Describes a vision target located somewhere on the field that your
|
||||
* SimVisionSystem can detect.
|
||||
*
|
||||
* @param targetPose Pose3d of the target in field-relative coordinates
|
||||
* @param targetWidth Width of the outer bounding box of the target.
|
||||
* @param targetHeight Pair Height of the outer bounding box of the
|
||||
* target.
|
||||
* @param targetId Id of the target
|
||||
*/
|
||||
SimVisionTarget(frc::Pose3d targetPose, units::meter_t targetWidth,
|
||||
units::meter_t targetHeight, int targetId)
|
||||
: targetPose(targetPose),
|
||||
targetWidth(targetWidth),
|
||||
targetHeight(targetHeight),
|
||||
targetArea(targetHeight * targetWidth),
|
||||
targetId(targetId) {}
|
||||
|
||||
frc::Pose3d targetPose;
|
||||
units::meter_t targetWidth;
|
||||
units::meter_t targetHeight;
|
||||
units::square_meter_t targetArea;
|
||||
int targetId;
|
||||
};
|
||||
} // namespace photon
|
||||
Reference in New Issue
Block a user