Class SnakeEyes
java.lang.Object
io.github.tigerbotics7125.tigerlib.vision.SnakeEyes
This class removes a lot of boiler plate code when designing vison logic, and allows for some
comfort features such as caching results.
-
Field Summary
Modifier and TypeFieldDescriptionprotected org.photonvision.targeting.PhotonPipelineResult
protected final org.photonvision.PhotonCamera
protected edu.wpi.first.math.geometry.Transform3d
Transformation from robots center (x, y), and the floor (z) -
Constructor Summary
-
Method Summary
Modifier and TypeMethodDescriptionvoid
addAprilTag
(int tagId, edu.wpi.first.math.geometry.Pose3d tagPose) Add an AprilTag to the vision systems known tag locations.org.photonvision.targeting.PhotonTrackedTarget
getBestTag
(org.photonvision.PhotonTargetSortMode sortMode, double ambiguityThreshold) edu.wpi.first.math.geometry.Rotation2d
getFaceTargetAngle
(org.photonvision.targeting.PhotonTrackedTarget targetToFace, edu.wpi.first.math.geometry.Pose2d robotPose) double
org.photonvision.targeting.PhotonPipelineResult
Returns and caches the latest result.edu.wpi.first.math.geometry.Pose3d
getRobotPose
(org.photonvision.targeting.PhotonTrackedTarget target) List<edu.wpi.first.math.geometry.Pose3d>
getRobotPoseEstimates
(double ambiguityThreshold) edu.wpi.first.math.geometry.Transform3d
edu.wpi.first.math.geometry.Pose3d
getTagPose
(int tagId) List<org.photonvision.targeting.PhotonTrackedTarget>
double
boolean
void
Blink the LEDs.void
Set the LEDs to their default mode.void
ledsOff()
Turn the LEDs off.void
ledsOn()
Turn the LEDs on.static void
Port forward the vision dashboard over rio usb connection.List<org.photonvision.targeting.PhotonTrackedTarget>
removeAmbiguousTags
(List<org.photonvision.targeting.PhotonTrackedTarget> tags, double ambiguityThreshold) Removes tags which have ambiguities larger than the provided threshold.void
setLeds
(org.photonvision.common.hardware.VisionLEDMode ledMode) Set the LEDs to the given mode.void
setPipeline
(int index) Set the pipeline via its index.
-
Field Details
-
mCam
protected final org.photonvision.PhotonCamera mCam -
mAprilTags
-
mRobotToCamera
protected edu.wpi.first.math.geometry.Transform3d mRobotToCameraTransformation from robots center (x, y), and the floor (z) -
mCachedResult
protected org.photonvision.targeting.PhotonPipelineResult mCachedResult
-
-
Constructor Details
-
SnakeEyes
-
SnakeEyes
public SnakeEyes(edu.wpi.first.networktables.NetworkTableInstance ntInstance, String cameraName, edu.wpi.first.math.geometry.Transform3d robotToCamera)
-
-
Method Details
-
portforward
public static void portforward()Port forward the vision dashboard over rio usb connection. -
addAprilTag
public void addAprilTag(int tagId, edu.wpi.first.math.geometry.Pose3d tagPose) Add an AprilTag to the vision systems known tag locations.- Parameters:
tagId
-tagPose
-
-
ledsOn
public void ledsOn()Turn the LEDs on. -
ledsOff
public void ledsOff()Turn the LEDs off. -
ledsBlink
public void ledsBlink()Blink the LEDs. -
ledsDefault
public void ledsDefault()Set the LEDs to their default mode. -
setLeds
public void setLeds(org.photonvision.common.hardware.VisionLEDMode ledMode) Set the LEDs to the given mode.- Parameters:
ledMode
-
-
setPipeline
public void setPipeline(int index) Set the pipeline via its index.- Parameters:
index
-
-
getRobotToCamera
public edu.wpi.first.math.geometry.Transform3d getRobotToCamera() -
getTagPose
public edu.wpi.first.math.geometry.Pose3d getTagPose(int tagId) - Parameters:
tagId
- The tag to get.- Returns:
- the Pose3d of the tag, null if tag has not been added.
-
getLatestResult
public org.photonvision.targeting.PhotonPipelineResult getLatestResult()Returns and caches the latest result. Allows users to make this call without performing actions on the returned value.- Returns:
- The latest result.
-
getTargets
- Returns:
- List of seen targets, if there are none, an empty list.
-
hasTargets
public boolean hasTargets()- Returns:
- If any targets are seen.
-
getLatency
public double getLatency()- Returns:
- The latency of the vision system in milli seconds.
-
getTimestamp
public double getTimestamp()- Returns:
- The timestamp of the latest result in seconds.
-
getRobotPose
public edu.wpi.first.math.geometry.Pose3d getRobotPose(org.photonvision.targeting.PhotonTrackedTarget target) - Parameters:
target
- The target to estimate robot pose by.- Returns:
- the Pose3d of the robot as known by the target.
- Throws:
IllegalArgumentException
- if target fiducial id is not in known tags map.
-
getFaceTargetAngle
public edu.wpi.first.math.geometry.Rotation2d getFaceTargetAngle(org.photonvision.targeting.PhotonTrackedTarget targetToFace, edu.wpi.first.math.geometry.Pose2d robotPose) - Parameters:
targetToFace
-robotPose
-- Returns:
- The heading the robot should be at if it were to face the target head-on.
-
removeAmbiguousTags
public List<org.photonvision.targeting.PhotonTrackedTarget> removeAmbiguousTags(List<org.photonvision.targeting.PhotonTrackedTarget> tags, double ambiguityThreshold) Removes tags which have ambiguities larger than the provided threshold.- Parameters:
tags
- List of tags to edit.ambiguityThreshold
- tags with ambiguity values greater than this will be removed.- Returns:
- The list of tags with ambiguous tags removed.
-
getBestTag
public org.photonvision.targeting.PhotonTrackedTarget getBestTag(org.photonvision.PhotonTargetSortMode sortMode, double ambiguityThreshold) - Parameters:
sortMode
-ambiguityThreshold
-- Returns:
- The best tag
-
getRobotPoseEstimates
- Parameters:
ambiguityThreshold
-- Returns:
- A list of estimated robot poses.
-