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

Return named type from PhotonPoseEstimator (#734)

Browse Source 
Adds PhotonPoseEstimator class, and deprecates RobotPoseEstimator
This commit is contained in:
Andrew Gasser
2023-01-14 09:06:15 -06:00
committed by GitHub
parent 073714f0bc
commit 357d8a518a
17 changed files with 1597 additions and 314 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java
View File

@@ -24,7 +24,6 @@
package frc.robot;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
@@ -49,6 +48,8 @@ import edu.wpi.first.wpilibj.simulation.EncoderSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Constants.DriveTrainConstants;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
/** Represents a differential drive style drivetrain. */
public class Drivetrain {
@@ -183,13 +184,14 @@ public class Drivetrain {
// Also apply vision measurements. We use 0.3 seconds in the past as an example
// -- on
// a real robot, this must be calculated based either on latency or timestamps.
Pair<Pose2d, Double> result =
Optional<EstimatedRobotPose> result =
pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition());
var camPose = result.getFirst();
var camPoseObsTime = result.getSecond();
if (camPose != null) {
m_poseEstimator.addVisionMeasurement(camPose, camPoseObsTime);
m_fieldSim.getObject("Cam Est Pos").setPose(camPose);
if (result.isPresent()) {
EstimatedRobotPose camPose = result.get();
m_poseEstimator.addVisionMeasurement(
camPose.estimatedPose.toPose2d(), camPose.timestampSeconds);
m_fieldSim.getObject("Cam Est Pos").setPose(camPose.estimatedPose.toPose2d());
} else {
// move it way off the screen to make it disappear
m_fieldSim.getObject("Cam Est Pos").setPose(new Pose2d(-100, -100, new Rotation2d()));

36
photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java
View File

@@ -26,23 +26,21 @@ package frc.robot;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.wpilibj.Timer;
import frc.robot.Constants.FieldConstants;
import frc.robot.Constants.VisionConstants;
import java.util.ArrayList;
import java.util.Optional;
import org.photonvision.EstimatedRobotPose;
import org.photonvision.PhotonCamera;
import org.photonvision.RobotPoseEstimator;
import org.photonvision.RobotPoseEstimator.PoseStrategy;
import org.photonvision.PhotonPoseEstimator;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
public class PhotonCameraWrapper {
public PhotonCamera photonCamera;
public RobotPoseEstimator robotPoseEstimator;
public PhotonPoseEstimator photonPoseEstimator;
public PhotonCameraWrapper() {
// Set up a test arena of two apriltags at the center of each driver station set
@@ -73,14 +71,10 @@ public class PhotonCameraWrapper {
.cameraName); // Change the name of your camera here to whatever it is in the
// PhotonVision UI.
// ... Add other cameras here
// Assemble the list of cameras & mount locations
var camList = new ArrayList<Pair<PhotonCamera, Transform3d>>();
camList.add(new Pair<PhotonCamera, Transform3d>(photonCamera, VisionConstants.robotToCam));
robotPoseEstimator =
new RobotPoseEstimator(atfl, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, camList);
// Create pose estimator
photonPoseEstimator =
new PhotonPoseEstimator(
atfl, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, photonCamera, VisionConstants.robotToCam);
}
/**
@@ -88,16 +82,8 @@ public class PhotonCameraWrapper {
* @return A pair of the fused camera observations to a single Pose2d on the field, and the time
* of the observation. Assumes a planar field and the robot is always firmly on the ground
*/
public Pair<Pose2d, Double> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
robotPoseEstimator.setReferencePose(prevEstimatedRobotPose);
double currentTime = Timer.getFPGATimestamp();
Optional<Pair<Pose3d, Double>> result = robotPoseEstimator.update();
if (result.isPresent()) {
return new Pair<Pose2d, Double>(
result.get().getFirst().toPose2d(), currentTime - result.get().getSecond());
} else {
return new Pair<Pose2d, Double>(null, 0.0);
}
public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
return photonPoseEstimator.update();
}
}