Skip to content

Commit d22d65a

Browse files
thakurnilesh307Harry695
thakurnilesh307
authored and
Harry695
committed
Add PhotonVision implementation
1 parent 943119e commit d22d65a

File tree

6 files changed

+374
-2
lines changed

6 files changed

+374
-2
lines changed

‎src/main/java/frc/robot/Constants.java

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,8 @@
44

55
package frc.robot;
66

7+
import edu.wpi.first.math.geometry.Transform3d;
8+
79
/**
810
* The Constants class provides a convenient place for teams to hold robot-wide
911
* numerical or boolean
@@ -31,6 +33,8 @@ public static class RobotConstants {
3133
*/
3234
public static final double WHEEL_DIAMETER_INCHES = 4.0;
3335

36+
public static final Transform3d APRILTAG_CAMERA_TO_ROBOT = new Transform3d();
37+
3438

3539
/**
3640
* Defines operator (i.e. driver and manipulator) constants.

‎src/main/java/frc/robot/RobotContainer.java

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -87,7 +87,8 @@ public void initShuffleboard(){
8787
m_autonomous.addShuffleboardLayout(operatorTab);
8888

8989
RobotPreferences.addShuffleBoardTab();
90-
90+
9191
m_subsystems.drivetrain.addShuffleboardTab();
92+
m_subsystems.aprilTag.addShuffleboardTab();
9293
}
9394
}

‎src/main/java/frc/robot/subsystems/AprilTagSubsystem.java

Lines changed: 152 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,152 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.subsystems;
6+
7+
import java.util.Optional;
8+
9+
import org.photonvision.targeting.PhotonTrackedTarget;
10+
11+
import com.nrg948.preferences.RobotPreferences;
12+
import com.nrg948.preferences.RobotPreferencesLayout;
13+
import com.nrg948.preferences.RobotPreferencesValue;
14+
15+
import edu.wpi.first.cscore.HttpCamera;
16+
import edu.wpi.first.cscore.HttpCamera.HttpCameraKind;
17+
import edu.wpi.first.cscore.VideoSource;
18+
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
19+
import edu.wpi.first.math.geometry.Pose3d;
20+
import edu.wpi.first.math.geometry.Rotation3d;
21+
import edu.wpi.first.math.geometry.Transform3d;
22+
import edu.wpi.first.math.geometry.Translation3d;
23+
import edu.wpi.first.util.datalog.DoubleLogEntry;
24+
import edu.wpi.first.wpilibj.DataLogManager;
25+
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
26+
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
27+
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
28+
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
29+
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
30+
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
31+
import frc.robot.Constants.RobotConstants;
32+
33+
/**
34+
* This subsystem is responsible for getting target information from
35+
* PhotonVision.
36+
*/
37+
38+
@RobotPreferencesLayout(groupName = "AprilTag", row = 1, column = 4, width = 2, height = 1)
39+
public class AprilTagSubsystem extends PhotonVisionSubsystemBase {
40+
41+
@RobotPreferencesValue
42+
public static final RobotPreferences.BooleanValue enableTab = new RobotPreferences.BooleanValue(
43+
"AprilTag", "Enable Tab", false);
44+
45+
private DoubleLogEntry targetXLogger = new DoubleLogEntry(DataLogManager.getLog(), "AprilTag/Target X");
46+
private DoubleLogEntry targetYLogger = new DoubleLogEntry(DataLogManager.getLog(), "AprilTag/Target Y");
47+
private DoubleLogEntry targetAngleLogger = new DoubleLogEntry(DataLogManager.getLog(), "AprilTag/Target Angle");
48+
49+
private SendableChooser<Integer> aprilTagIdChooser = new SendableChooser<Integer>();
50+
51+
/** Creates a new PhotonVisionSubsystem. */
52+
public AprilTagSubsystem() {
53+
super("AprilTagCamera", RobotConstants.APRILTAG_CAMERA_TO_ROBOT);
54+
55+
for (int i = 1; i <= 16; i++) {
56+
aprilTagIdChooser.addOption(String.valueOf(i), i);
57+
}
58+
}
59+
60+
@Override
61+
public void updatePoseEstimate(SwerveDrivePoseEstimator estimator, Pose3d targetPose) {
62+
if (hasTargets()) {
63+
Transform3d cameraToRobot = getCameraToRobotTransform();
64+
Transform3d targetToCamera = new Transform3d(
65+
new Translation3d(
66+
getDistanceToBestTarget(),
67+
new Rotation3d(0, 0, Math.toRadians(-getAngleToBestTarget()))),
68+
cameraToRobot.getRotation()).inverse();
69+
Pose3d cameraPose = targetPose.transformBy(targetToCamera);
70+
Pose3d robotPose = cameraPose.transformBy(cameraToRobot);
71+
72+
estimator.addVisionMeasurement(robotPose.toPose2d(), getTargetTimestamp());
73+
}
74+
75+
targetXLogger.append(targetPose.getX());
76+
targetYLogger.append(targetPose.getY());
77+
targetAngleLogger.append(Math.toRadians(targetPose.getRotation().getAngle()));
78+
}
79+
80+
/**
81+
* Returns the AprilTag target of the input ID.
82+
*
83+
* @param id The AprilTag ID.
84+
* @return The target with the input ID.
85+
*/
86+
public Optional<PhotonTrackedTarget> getTarget(int id) {
87+
return getTargets().stream().filter(target -> target.getFiducialId() == id).findFirst();
88+
}
89+
90+
/**
91+
* Returns the transform from the camera to the AprilTag with input ID.
92+
* @param id The AprilTag ID.
93+
* @return The transform from the camera to the AprilTag with input ID.
94+
*/
95+
public Transform3d getCameraToTarget(int id) {
96+
return getTarget(id).get().getBestCameraToTarget();
97+
}
98+
99+
/**
100+
* Returns the distance to the target with the input ID. Returns 0 if target not found.
101+
*
102+
* @param id The AprilTag ID.
103+
* @return The distance to the target with the input ID.
104+
*/
105+
public double getDistanceToTarget(int id) {
106+
Optional<PhotonTrackedTarget> target = getTarget(id);
107+
if (target.isEmpty()) {
108+
return 0.0;
109+
}
110+
var bestCameraToTarget = target.get().getBestCameraToTarget();
111+
return Math.hypot(bestCameraToTarget.getX(), bestCameraToTarget.getY());
112+
}
113+
114+
/**
115+
* Returns the angle to the target with the input ID. Returns 0 if target not found.
116+
*
117+
* @param id The AprilTag ID.
118+
* @return The angle to the target with the input ID.
119+
*/
120+
public double getAngleToTarget(int id) {
121+
Optional<PhotonTrackedTarget> target = getTarget(id);
122+
if (target.isEmpty()) {
123+
return 0.0;
124+
}
125+
return target.get().getYaw();
126+
}
127+
128+
/**
129+
* Adds a tab for April Tag in Shuffleboard.
130+
*/
131+
public void addShuffleboardTab() {
132+
if (!enableTab.getValue()) {
133+
return;
134+
}
135+
136+
ShuffleboardTab visionTab = Shuffleboard.getTab("April Tag");
137+
ShuffleboardLayout targetLayout = visionTab.getLayout("Target Info", BuiltInLayouts.kList)
138+
.withPosition(0, 0)
139+
.withSize(2, 5);
140+
targetLayout.add("Id Selection", aprilTagIdChooser).withWidget(BuiltInWidgets.kComboBoxChooser);
141+
targetLayout.addBoolean("Has Target", this::hasTargets);
142+
targetLayout.addDouble("Distance", () -> getDistanceToTarget(aprilTagIdChooser.getSelected()));
143+
targetLayout.addDouble("Angle", () -> getAngleToTarget(aprilTagIdChooser.getSelected()));
144+
145+
VideoSource video = new HttpCamera("photonvision_Port_1182_MJPEG_Server", "http://10.9.48.11:1182/?action=stream",
146+
HttpCameraKind.kMJPGStreamer);
147+
visionTab.add("April Tag", video)
148+
.withWidget(BuiltInWidgets.kCameraStream)
149+
.withPosition(2, 0)
150+
.withSize(4, 3);
151+
}
152+
}

‎src/main/java/frc/robot/subsystems/PhotonVisionSubsystemBase.java

Lines changed: 158 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,158 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
package frc.robot.subsystems;
6+
7+
import java.util.List;
8+
9+
import org.photonvision.PhotonCamera;
10+
import org.photonvision.targeting.PhotonPipelineResult;
11+
import org.photonvision.targeting.PhotonTrackedTarget;
12+
13+
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
14+
import edu.wpi.first.math.geometry.Pose3d;
15+
import edu.wpi.first.math.geometry.Transform3d;
16+
import edu.wpi.first.util.datalog.BooleanLogEntry;
17+
import edu.wpi.first.util.datalog.DoubleLogEntry;
18+
import edu.wpi.first.wpilibj.DataLogManager;
19+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
20+
21+
/**
22+
* This is a base class for subsystems responsible for getting target
23+
* information from PhotonVision.
24+
*/
25+
public abstract class PhotonVisionSubsystemBase extends SubsystemBase {
26+
27+
private final PhotonCamera camera;
28+
private final Transform3d cameraToRobot;
29+
private final Transform3d robotToCamera;
30+
private PhotonPipelineResult result = new PhotonPipelineResult();
31+
32+
private BooleanLogEntry hasTargetLogger;
33+
private DoubleLogEntry distanceLogger;
34+
private DoubleLogEntry angleLogger;
35+
36+
/**
37+
* Creates a new PhotonVisionSubsystemBase.
38+
*
39+
* @param cameraName The name of the PhotonVision camera.
40+
* @param cameraToRobot The transform from the camera to center of the robot.
41+
*/
42+
public PhotonVisionSubsystemBase(String cameraName, Transform3d cameraToRobot) {
43+
this.camera = new PhotonCamera(cameraName);
44+
this.cameraToRobot = cameraToRobot;
45+
this.robotToCamera = cameraToRobot.inverse();
46+
47+
hasTargetLogger = new BooleanLogEntry(DataLogManager.getLog(), String.format("/%s/Has Target", cameraName));
48+
distanceLogger = new DoubleLogEntry(DataLogManager.getLog(), String.format("/%s/Distance", cameraName));
49+
angleLogger = new DoubleLogEntry(DataLogManager.getLog(), String.format("/%s/Angle", cameraName));
50+
}
51+
52+
@Override
53+
public void periodic() {
54+
PhotonPipelineResult currentResult = camera.getLatestResult();
55+
56+
if (this.result.hasTargets() != currentResult.hasTargets()) {
57+
hasTargetLogger.append(currentResult.hasTargets());
58+
}
59+
60+
this.result = currentResult;
61+
62+
if (hasTargets()) {
63+
distanceLogger.append(getDistanceToBestTarget());
64+
angleLogger.append(-getAngleToBestTarget());
65+
}
66+
}
67+
68+
/**
69+
* Updates the pose estimate based on vision information using the expected
70+
* target pose.
71+
*
72+
* @param estimator The pose estimator.
73+
* @param targetPose The expected pose of the target in the field of vision. The
74+
* pose is specified in field-relative coordinates.
75+
*/
76+
public abstract void updatePoseEstimate(SwerveDrivePoseEstimator estimator, Pose3d targetPose);
77+
78+
/**
79+
* Returns the transform from the camera to center of the robot.
80+
*
81+
* @return The transform from the camera to center of the robot.
82+
*/
83+
public Transform3d getCameraToRobotTransform() {
84+
return cameraToRobot;
85+
}
86+
87+
/**
88+
* Returns the transform from the center of the robot to the camera.
89+
*
90+
* @return The transform from the center of the robot to the camera.
91+
*/
92+
public Transform3d getRobotToCameraTransform() {
93+
return robotToCamera;
94+
}
95+
96+
/**
97+
* Returns whether the result contains any targets.
98+
*
99+
* @return Returns true if there are targets.
100+
*/
101+
public boolean hasTargets() {
102+
return result.hasTargets();
103+
}
104+
105+
/**
106+
* Returns information on the best target.
107+
*
108+
* @return Information on the best target.
109+
*/
110+
public PhotonTrackedTarget getBestTarget() {
111+
return result.getBestTarget();
112+
}
113+
114+
/**
115+
* Returns the distance to the best target.
116+
*
117+
* @return The distance, in meters, to the best target.
118+
*/
119+
public double getDistanceToBestTarget() {
120+
if (!hasTargets()) {
121+
return 0;
122+
}
123+
124+
Transform3d bestTarget = getBestTarget().getBestCameraToTarget();
125+
return Math.hypot(bestTarget.getX(), bestTarget.getY());
126+
}
127+
128+
/**
129+
* Returns the angle to the best target.
130+
*
131+
* @return The angle to the best target.
132+
*/
133+
public double getAngleToBestTarget() {
134+
if (!hasTargets()) {
135+
return 0;
136+
}
137+
138+
return getBestTarget().getYaw();
139+
}
140+
141+
/**
142+
* Returns the estimated time, in seconds, the target was detected.
143+
*
144+
* @return The timestamp in seconds or -1 if no target was detected.
145+
*/
146+
public double getTargetTimestamp() {
147+
return result.getTimestampSeconds();
148+
}
149+
150+
/**
151+
* Returns a list of visible targets.
152+
*
153+
* @return A list of visible targets.
154+
*/
155+
public List<PhotonTrackedTarget> getTargets() {
156+
return result.getTargets();
157+
}
158+
}

‎src/main/java/frc/robot/subsystems/Subsystems.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,5 +7,5 @@
77
/** Add your docs here. */
88
public class Subsystems {
99
public SwerveSubsystem drivetrain = new SwerveSubsystem();
10-
10+
public AprilTagSubsystem aprilTag = new AprilTagSubsystem();
1111
}

0 commit comments

Comments
 (0)