diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 57389bf0f..942cd1c9f 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -67,7 +67,7 @@ public class PhotonPoseEstimator { private AprilTagFieldLayout fieldTags; private PoseStrategy strategy; private final PhotonCamera camera; - private final Transform3d robotToCamera; + private Transform3d robotToCamera; private Pose3d lastPose; private Pose3d referencePose; @@ -183,6 +183,21 @@ public class PhotonPoseEstimator { setLastPose(new Pose3d(lastPose)); } + /** @return The current transform from the center of the robot to the camera mount position */ + public Transform3d getRobotToCameraTransform() { + return robotToCamera; + } + + /** + * Useful for pan and tilt mechanisms and such. + * + * @param robotToCamera The current transform from the center of the robot to the camera mount + * position + */ + public void setRobotToCameraTransform(Transform3d robotToCamera) { + this.robotToCamera = robotToCamera; + } + /** * Poll data from the configured cameras and update the estimated position of the robot. Returns * empty if there are no cameras set or no targets were found from the cameras. diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h index 16e18b9ae..6407732ee 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h @@ -123,6 +123,24 @@ class PhotonPoseEstimator { 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.