Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-24 01:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[photon-targeting] Move C++ targeting classes to photon-targetting (#1009)

Browse Source 
* 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
2023-11-19 15:16:22 -05:00
committed by GitHub
parent 308fd801d4
commit 994ea1e76b
50 changed files with 1132 additions and 910 deletions
Download Patch File Download Diff File Expand all files Collapse all files

214
photon-lib/src/main/native/include/photon/PhotonCamera.h Normal file
View File

@@ -0,0 +1,214 @@
/*
* 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 <memory>
#include <string>
#include <networktables/BooleanTopic.h>
#include <networktables/DoubleArrayTopic.h>
#include <networktables/DoubleTopic.h>
#include <networktables/IntegerTopic.h>
#include <networktables/MultiSubscriber.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
#include <networktables/RawTopic.h>
#include <networktables/StringTopic.h>
#include <units/time.h>
#include <wpi/deprecated.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 cv {
class Mat;
} // namespace cv
namespace photon {
enum LEDMode : int { kDefault = -1, kOff = 0, kOn = 1, kBlink = 2 };
/**
* Represents a camera that is connected to PhotonVision.ß
*/
class PhotonCamera {
public:
/**
* Constructs a PhotonCamera from a root table.
*
* @param instance The NetworkTableInstance to pull data from. This can be a
* custom instance in simulation, but should *usually* be the default
* NTInstance from {@link NetworkTableInstance::getDefault}
* @param cameraName The name of the camera, as seen in the UI.
* over.
*/
explicit PhotonCamera(nt::NetworkTableInstance instance,
const std::string_view cameraName);
/**
* Constructs a PhotonCamera from the name of the camera.
* @param cameraName The nickname of the camera (found in the PhotonVision
* UI).
*/
explicit PhotonCamera(const std::string_view cameraName);
PhotonCamera(PhotonCamera&&) = default;
virtual ~PhotonCamera() = default;
/**
* Returns the latest pipeline result.
* @return The latest pipeline result.
*/
virtual PhotonPipelineResult GetLatestResult();
/**
* Toggles driver mode.
* @param driverMode Whether to set driver mode.
*/
void SetDriverMode(bool driverMode);
/**
* Returns whether the camera is in driver mode.
* @return Whether the camera is in driver mode.
*/
bool GetDriverMode() const;
/**
* Request the camera to save a new image file from the input
* camera stream with overlays.
* Images take up space in the filesystem of the PhotonCamera.
* Calling it frequently will fill up disk space and eventually
* cause the system to stop working.
* Clear out images in /opt/photonvision/photonvision_config/imgSaves
* frequently to prevent issues.
*/
void TakeInputSnapshot(void);
/**
* Request the camera to save a new image file from the output
* stream with overlays.
* Images take up space in the filesystem of the PhotonCamera.
* Calling it frequently will fill up disk space and eventually
* cause the system to stop working.
* Clear out images in /opt/photonvision/photonvision_config/imgSaves
* frequently to prevent issues.
*/
void TakeOutputSnapshot(void);
/**
* Allows the user to select the active pipeline index.
* @param index The active pipeline index.
*/
void SetPipelineIndex(int index);
/**
* Returns the active pipeline index.
* @return The active pipeline index.
*/
int GetPipelineIndex() const;
/**
* Returns the current LED mode.
* @return The current LED mode.
*/
LEDMode GetLEDMode() const;
/**
* Sets the LED mode.
* @param led The mode to set to.
*/
void SetLEDMode(LEDMode led);
/**
* Returns the name of the camera.
* This will return the same value that was given to the constructor as
* cameraName.
* @return The name of the camera.
*/
const std::string_view GetCameraName() const;
std::optional<cv::Mat> GetCameraMatrix();
std::optional<cv::Mat> GetDistCoeffs();
/**
* Returns whether the latest target result has targets.
* This method is deprecated; {@link PhotonPipelineResult#hasTargets()} should
* be used instead.
* @deprecated This method should be replaced with {@link
* PhotonPipelineResult#HasTargets()}
* @return Whether the latest target result has targets.
*/
WPI_DEPRECATED(
"This method should be replaced with PhotonPipelineResult::HasTargets()")
bool HasTargets() { return GetLatestResult().HasTargets(); }
inline static void SetVersionCheckEnabled(bool enabled) {
PhotonCamera::VERSION_CHECK_ENABLED = enabled;
}
// For use in tests
bool test = false;
PhotonPipelineResult testResult;
protected:
std::shared_ptr<nt::NetworkTable> mainTable;
std::shared_ptr<nt::NetworkTable> rootTable;
nt::RawSubscriber rawBytesEntry;
nt::IntegerPublisher inputSaveImgEntry;
nt::IntegerSubscriber inputSaveImgSubscriber;
nt::IntegerPublisher outputSaveImgEntry;
nt::IntegerSubscriber outputSaveImgSubscriber;
nt::IntegerPublisher pipelineIndexPub;
nt::IntegerSubscriber pipelineIndexSub;
nt::IntegerPublisher ledModePub;
nt::IntegerSubscriber ledModeSub;
nt::StringSubscriber versionEntry;
nt::DoubleArraySubscriber cameraIntrinsicsSubscriber;
nt::DoubleArraySubscriber cameraDistortionSubscriber;
nt::BooleanSubscriber driverModeSubscriber;
nt::BooleanPublisher driverModePublisher;
nt::IntegerSubscriber ledModeSubscriber;
nt::MultiSubscriber m_topicNameSubscriber;
std::string path;
std::string m_cameraName;
mutable Packet packet;
private:
units::second_t lastVersionCheckTime = 0_s;
inline static bool VERSION_CHECK_ENABLED = true;
void VerifyVersion();
};
} // namespace photon

303
photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h Normal file
View File

@@ -0,0 +1,303 @@
/*
* 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 <memory>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Transform3d.h>
#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"
namespace cv {
class Mat;
} // namespace cv
namespace photon {
enum PoseStrategy {
LOWEST_AMBIGUITY = 0,
CLOSEST_TO_CAMERA_HEIGHT,
CLOSEST_TO_REFERENCE_POSE,
CLOSEST_TO_LAST_POSE,
AVERAGE_BEST_TARGETS,
MULTI_TAG_PNP_ON_COPROCESSOR,
MULTI_TAG_PNP_ON_RIO,
};
struct EstimatedRobotPose {
/** The estimated pose */
frc::Pose3d estimatedPose;
/** The estimated time the frame used to derive the robot pose was taken, in
* the same timebase as the RoboRIO FPGA Timestamp */
units::second_t timestamp;
/** A list of the targets used to compute this pose */
wpi::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
/** The strategy actually used to produce this pose */
PoseStrategy strategy;
EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_,
std::span<const PhotonTrackedTarget> targets,
PoseStrategy strategy_)
: estimatedPose(pose_),
timestamp(time_),
targetsUsed(targets.data(), targets.data() + targets.size()),
strategy(strategy_) {}
};
/**
* The PhotonPoseEstimator class filters or combines readings from all the
* fiducials visible at a given timestamp on the field to produce a single robot
* in field pose, using the strategy set below. Example usage can be found in
* our apriltagExample example project.
*/
class PhotonPoseEstimator {
public:
/**
* Create a new PhotonPoseEstimator.
*
* <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
* <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
* at (1.0,2.0,3.0) </code> }
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param camera PhotonCameras and
* @param robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
*/
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
PoseStrategy strategy, PhotonCamera&& camera,
frc::Transform3d robotToCamera);
/**
* Create a new PhotonPoseEstimator.
*
* <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
* <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
* at (1.0,2.0,3.0) </code> }
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
*/
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
PoseStrategy strategy,
frc::Transform3d robotToCamera);
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
* @return the AprilTagFieldLayout
*/
frc::AprilTagFieldLayout GetFieldLayout() const { return aprilTags; }
/**
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
*/
PoseStrategy GetPoseStrategy() const { return strategy; }
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
*/
inline void SetPoseStrategy(PoseStrategy strat) {
if (strategy != strat) {
InvalidatePoseCache();
}
strategy = strat;
}
/**
* Set the Position Estimation Strategy used in multi-tag mode when
* only one tag can be seen. Must NOT be MULTI_TAG_PNP
*
* @param strategy the strategy to set
*/
void SetMultiTagFallbackStrategy(PoseStrategy strategy);
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
*/
frc::Pose3d GetReferencePose() const { return referencePose; }
/**
* Update the stored reference pose for use when using the
* CLOSEST_TO_REFERENCE_POSE strategy.
*
* @param referencePose the referencePose to set
*/
inline void SetReferencePose(frc::Pose3d referencePose) {
if (this->referencePose != referencePose) {
InvalidatePoseCache();
}
this->referencePose = referencePose;
}
/**
* @return The current transform from the center of the robot to the camera
* mount position.
*/
inline frc::Transform3d GetRobotToCameraTransform() {
return m_robotToCamera;
}
/**
* Useful for pan and tilt mechanisms, or cameras on turrets
*
* @param robotToCamera The current transform from the center of the robot to
* the camera mount position.
*/
inline void SetRobotToCameraTransform(frc::Transform3d robotToCamera) {
m_robotToCamera = robotToCamera;
}
/**
* Update the stored last pose. Useful for setting the initial estimate when
* using the CLOSEST_TO_LAST_POSE strategy.
*
* @param lastPose the lastPose to set
*/
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
/**
* Update the pose estimator. Internally grabs a new PhotonPipelineResult from
* the camera and process it.
*/
std::optional<EstimatedRobotPose> Update();
/**
* Update the pose estimator.
*/
std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result);
/**
* Update the pose estimator.
*/
std::optional<EstimatedRobotPose> Update(
const PhotonPipelineResult& result,
std::optional<cv::Mat> cameraMatrixData,
std::optional<cv::Mat> coeffsData);
inline std::shared_ptr<PhotonCamera> GetCamera() { return camera; }
private:
frc::AprilTagFieldLayout aprilTags;
PoseStrategy strategy;
PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY;
std::shared_ptr<PhotonCamera> camera;
frc::Transform3d m_robotToCamera;
frc::Pose3d lastPose;
frc::Pose3d referencePose;
units::second_t poseCacheTimestamp;
inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
std::optional<EstimatedRobotPose> Update(
PhotonPipelineResult result, std::optional<cv::Mat> cameraMatrixData,
std::optional<cv::Mat> coeffsData, PoseStrategy strategy);
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> LowestAmbiguityStrategy(
PhotonPipelineResult result);
/**
* Return the estimated position of the robot using the target with the lowest
* delta height difference between the estimated and actual height of the
* camera.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> ClosestToCameraHeightStrategy(
PhotonPipelineResult result);
/**
* Return the estimated position of the robot using the target with the lowest
* delta in the vector magnitude between it and the reference pose.
*
* @param referencePose reference pose to check vector magnitude difference
* against.
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
PhotonPipelineResult result);
/**
* Return the pose calculated by combining all tags into one on coprocessor
*
* @return the estimated position of the robot in the FCS
*/
std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs);
/**
* Return the pose calculation using all targets in view in the same PNP()
calculation
*
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs);
/**
* Return the average of the best target poses using ambiguity as weight.
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
PhotonPipelineResult result);
};
} // namespace photon

89
photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h Normal file
View File

@@ -0,0 +1,89 @@
/*
* 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 <cmath>
#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 {
namespace PhotonTargetSortMode {
struct Smallest {
inline bool operator()(const PhotonTrackedTarget& target1,
const PhotonTrackedTarget& target2) {
return target1.GetArea() < target2.GetArea();
}
};
struct Largest {
inline bool operator()(const PhotonTrackedTarget& target1,
const PhotonTrackedTarget& target2) {
return target1.GetArea() > target2.GetArea();
}
};
struct Highest {
inline bool operator()(const PhotonTrackedTarget& target1,
const PhotonTrackedTarget& target2) {
return target1.GetPitch() < target2.GetPitch();
}
};
struct Lowest {
inline bool operator()(const PhotonTrackedTarget& target1,
const PhotonTrackedTarget& target2) {
return target1.GetPitch() > target2.GetPitch();
}
};
struct RightMost {
inline bool operator()(const PhotonTrackedTarget& target1,
const PhotonTrackedTarget& target2) {
return target1.GetYaw() < target2.GetYaw();
}
};
struct LeftMost {
inline bool operator()(const PhotonTrackedTarget& target1,
const PhotonTrackedTarget& target2) {
return target1.GetYaw() > target2.GetYaw();
}
};
struct CenterMost {
inline bool operator()(const PhotonTrackedTarget& target1,
const PhotonTrackedTarget& target2) {
return std::pow(target1.GetPitch(), 2) + std::pow(target1.GetYaw(), 2) <
std::pow(target2.GetPitch(), 2) + std::pow(target2.GetYaw(), 2);
}
};
} // namespace PhotonTargetSortMode
} // namespace photon

186
photon-lib/src/main/native/include/photon/PhotonUtils.h Normal file
View File

@@ -0,0 +1,186 @@
/*
* 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/Pose2d.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/geometry/Transform2d.h>
#include <frc/geometry/Translation2d.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/math.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 PhotonUtils {
public:
/**
* Algorithm from
* https://docs.limelightvision.io/en/latest/cs_estimating_distance.html
* Estimates range to a target using the target's elevation. This method can
* produce more stable results than SolvePNP when well tuned, if the full 6d
* robot pose is not required.
*
* @param cameraHeight The height of the camera off the floor.
* @param targetHeight The height of the target off the floor.
* @param cameraPitch The pitch of the camera from the horizontal plane.
* Positive valueBytes up.
* @param targetPitch The pitch of the target in the camera's lens. Positive
* values up.
* @return The estimated distance to the target.
*/
static units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight,
units::meter_t targetHeight,
units::radian_t cameraPitch,
units::radian_t targetPitch) {
return (targetHeight - cameraHeight) /
units::math::tan(cameraPitch + targetPitch);
}
/**
* Estimate the Translation2d of the target relative to the camera.
*
* @param targetDistance The distance to the target.
* @param yaw The observed yaw of the target.
*
* @return The target's camera-relative translation.
*/
static frc::Translation2d EstimateCameraToTargetTranslation(
units::meter_t targetDistance, const frc::Rotation2d& yaw) {
return {targetDistance * yaw.Cos(), targetDistance * yaw.Sin()};
}
/**
* Estimate the position of the robot in the field.
*
* @param cameraHeightMeters The physical height of the camera off the floor
* in meters.
* @param targetHeightMeters The physical height of the target off the floor
* in meters. This should be the height of whatever is being targeted (i.e. if
* the targeting region is set to top, this should be the height of the top of
* the target).
* @param cameraPitchRadians The pitch of the camera from the horizontal plane
* in radians. Positive values up.
* @param targetPitchRadians The pitch of the target in the camera's lens in
* radians. Positive values up.
* @param targetYaw The observed yaw of the target. Note that this
* *must* be CCW-positive, and Photon returns
* CW-positive.
* @param gyroAngle The current robot gyro angle, likely from
* odometry.
* @param fieldToTarget A frc::Pose2d representing the target position in
* the field coordinate system.
* @param cameraToRobot The position of the robot relative to the camera.
* If the camera was mounted 3 inches behind the
* "origin" (usually physical center) of the robot,
* this would be frc::Transform2d(3 inches, 0
* inches, 0 degrees).
* @return The position of the robot in the field.
*/
static frc::Pose2d EstimateFieldToRobot(
units::meter_t cameraHeight, units::meter_t targetHeight,
units::radian_t cameraPitch, units::radian_t targetPitch,
const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle,
const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot) {
return EstimateFieldToRobot(
EstimateCameraToTarget(
EstimateCameraToTargetTranslation(
CalculateDistanceToTarget(cameraHeight, targetHeight,
cameraPitch, targetPitch),
targetYaw),
fieldToTarget, gyroAngle),
fieldToTarget, cameraToRobot);
}
/**
* Estimates a {@link frc::Transform2d} that maps the camera position to the
* target position, using the robot's gyro. Note that the gyro angle provided
* *must* line up with the field coordinate system -- that is, it should read
* zero degrees when pointed towards the opposing alliance station, and
* increase as the robot rotates CCW.
*
* @param cameraToTargetTranslation A Translation2d that encodes the x/y
* position of the target relative to the
* camera.
* @param fieldToTarget A frc::Pose2d representing the target
* position in the field coordinate system.
* @param gyroAngle The current robot gyro angle, likely from
* odometry.
* @return A frc::Transform2d that takes us from the camera to the target.
*/
static frc::Transform2d EstimateCameraToTarget(
const frc::Translation2d& cameraToTargetTranslation,
const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle) {
// This pose maps our camera at the origin out to our target, in the robot
// reference frame
// The translation part of this frc::Transform2d is from the above step, and
// the rotation uses our robot's gyro.
return frc::Transform2d(cameraToTargetTranslation,
-gyroAngle - fieldToTarget.Rotation());
}
/**
* Estimates the pose of the robot in the field coordinate system, given the
* position of the target relative to the camera, the target relative to the
* field, and the robot relative to the camera.
*
* @param cameraToTarget The position of the target relative to the camera.
* @param fieldToTarget The position of the target in the field.
* @param cameraToRobot The position of the robot relative to the camera. If
* the camera was mounted 3 inches behind the "origin"
* (usually physical center) of the robot, this would be
* frc::Transform2d(3 inches, 0 inches, 0 degrees).
* @return The position of the robot in the field.
*/
static frc::Pose2d EstimateFieldToRobot(
const frc::Transform2d& cameraToTarget, const frc::Pose2d& fieldToTarget,
const frc::Transform2d& cameraToRobot) {
return EstimateFieldToCamera(cameraToTarget, fieldToTarget)
.TransformBy(cameraToRobot);
}
/**
* Estimates the pose of the camera in the field coordinate system, given the
* position of the target relative to the camera, and the target relative to
* the field. This *only* tracks the position of the camera, not the position
* of the robot itself.
*
* @param cameraToTarget The position of the target relative to the camera.
* @param fieldToTarget The position of the target in the field.
* @return The position of the camera in the field.
*/
static frc::Pose2d EstimateFieldToCamera(
const frc::Transform2d& cameraToTarget,
const frc::Pose2d& fieldToTarget) {
auto targetToCamera = cameraToTarget.Inverse();
return fieldToTarget.TransformBy(targetToCamera);
}
};
} // namespace photon

137
photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h Normal file
View File

@@ -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

226
photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h Normal file
View File

@@ -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

65
photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h Normal file
View File

@@ -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