Create C++ Apriltag example (#794)

* Create C++ Apriltag example * Delete libphotonlibcamera.so * Update PhotonCameraWrapper.h * Delete extra files Update .gitignore
2026-06-19 00:41:41 +00:00 · 2023-02-12 00:07:07 -05:00
parent 3edc8750ec
commit a2dfe48679
26 changed files with 1040 additions and 10 deletions
--- a/photonlib-cpp-examples/apriltagExample/src/main/cpp/Drivetrain.cpp
+++ b/photonlib-cpp-examples/apriltagExample/src/main/cpp/Drivetrain.cpp
@@ -0,0 +1,92 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 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.
+ */
+
+#include "Drivetrain.h"
+
+#include <frc/RobotController.h>
+#include <frc/smartdashboard/Field2d.h>
+
+void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
+  auto leftFeedforward = m_feedforward.Calculate(speeds.left);
+  auto rightFeedforward = m_feedforward.Calculate(speeds.right);
+  double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(),
+                                                    speeds.left.value());
+  double rightOutput = m_rightPIDController.Calculate(m_rightEncoder.GetRate(),
+                                                      speeds.right.value());
+
+  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
+}
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+                       units::radians_per_second_t rot) {
+  SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
+}
+
+void Drivetrain::UpdateOdometry() {
+  m_poseEstimator.Update(m_gyro.GetRotation2d(),
+                         units::meter_t{m_leftEncoder.GetDistance()},
+                         units::meter_t{m_rightEncoder.GetDistance()});
+}
+
+void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+  m_drivetrainSimulator.SetPose(pose);
+  m_poseEstimator.ResetPosition(
+      m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
+      units::meter_t{m_rightEncoder.GetDistance()}, pose);
+}
+
+void Drivetrain::SimulationPeriodic() {
+  // To update our simulation, we set motor voltage inputs, update the
+  // simulation, and write the simulated positions and velocities to our
+  // simulated encoder and gyro. We negate the right side so that positive
+  // voltages make the right side move forward.
+
+  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftGroup.Get()} *
+                                      frc::RobotController::GetInputVoltage(),
+                                  units::volt_t{m_rightGroup.Get()} *
+                                      frc::RobotController::GetInputVoltage());
+  m_drivetrainSimulator.Update(20_ms);
+
+  m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
+  m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
+  m_rightEncoderSim.SetDistance(
+      m_drivetrainSimulator.GetRightPosition().value());
+  m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
+  m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value());
+}
+
+void Drivetrain::Periodic() {
+  UpdateOdometry();
+
+  auto result = m_pcw.Update(m_poseEstimator.GetEstimatedPosition());
+  if (result) {
+    m_poseEstimator.AddVisionMeasurement(
+        result.value().estimatedPose.ToPose2d(), result.value().timestamp);
+  }
+
+  m_fieldSim.SetRobotPose(m_poseEstimator.GetEstimatedPosition());
+}
--- a/photonlib-cpp-examples/apriltagExample/src/main/cpp/Robot.cpp
+++ b/photonlib-cpp-examples/apriltagExample/src/main/cpp/Robot.cpp
@@ -0,0 +1,61 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 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.
+ */
+
+#include "Robot.h"
+
+#include <frc/RobotBase.h>
+#include <networktables/NetworkTable.h>
+
+void Robot::RobotInit() {
+  if constexpr (frc::RobotBase::IsSimulation()) {
+    auto inst = nt::NetworkTableInstance::GetDefault();
+    inst.StopServer();
+    // set the NT server if simulating this code.
+    // "localhost" for photon on desktop, or "photonvision.local" or
+    // "[ip-address]" for coprocessor
+    inst.SetServer("localhost");
+    inst.StartClient4("Robot Simulation");
+  }
+}
+
+void Robot::TeleopPeriodic() {
+  // Get the x speed. We are inverting this because Xbox controllers return
+  // negative values when we push forward.
+  const auto xSpeed = -m_controller.GetLeftY() * Drivetrain::kMaxSpeed;
+
+  // Get the rate of angular rotation. We are inverting this because we want a
+  // positive value when we pull to the left (remember, CCW is positive in
+  // mathematics). Xbox controllers return positive values when you pull to
+  // the right by default.
+  auto rot = -m_controller.GetRightX() * Drivetrain::kMaxAngularSpeed;
+
+  m_drive.Drive(xSpeed, rot);
+}
+
+void Robot::RobotPeriodic() { m_drive.Periodic(); }
+void Robot::SimulationPeriodic() { m_drive.SimulationPeriodic(); }
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
--- a/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h
+++ b/photonlib-cpp-examples/apriltagExample/src/main/include/Drivetrain.h
@@ -0,0 +1,140 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 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 <numbers>
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/estimator/DifferentialDrivePoseEstimator.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/AnalogGyroSim.h>
+#include <frc/simulation/DifferentialDrivetrainSim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/smartdashboard/Field2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
+
+#include "PhotonCameraWrapper.h"
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+  Drivetrain() {
+    m_gyro.Reset();
+
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightGroup.SetInverted(true);
+
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
+                                      kEncoderResolution);
+    m_rightEncoder.SetDistancePerPulse(2 * std::numbers::pi * kWheelRadius /
+                                       kEncoderResolution);
+
+    m_leftEncoder.Reset();
+    m_rightEncoder.Reset();
+
+    m_rightGroup.SetInverted(true);
+
+    frc::SmartDashboard::PutData("Field", &m_fieldSim);
+  }
+
+  static constexpr units::meters_per_second_t kMaxSpeed =
+      3.0_mps;  // 3 meters per second
+  static constexpr units::radians_per_second_t kMaxAngularSpeed{
+      std::numbers::pi};  // 1/2 rotation per second
+
+  void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
+  void Drive(units::meters_per_second_t xSpeed,
+             units::radians_per_second_t rot);
+  void UpdateOdometry();
+  void ResetOdometry(const frc::Pose2d& pose);
+
+  frc::Pose2d GetPose() const { return m_poseEstimator.GetEstimatedPosition(); }
+
+  void SimulationPeriodic();
+  void Periodic();
+
+ private:
+  static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
+  static constexpr double kWheelRadius = 0.0508;  // meters
+  static constexpr int kEncoderResolution = 4096;
+
+  frc::PWMSparkMax m_leftLeader{1};
+  frc::PWMSparkMax m_leftFollower{2};
+  frc::PWMSparkMax m_rightLeader{3};
+  frc::PWMSparkMax m_rightFollower{4};
+
+  frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
+  frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
+
+  frc::Encoder m_leftEncoder{0, 1};
+  frc::Encoder m_rightEncoder{2, 3};
+
+  frc2::PIDController m_leftPIDController{8.5, 0.0, 0.0};
+  frc2::PIDController m_rightPIDController{8.5, 0.0, 0.0};
+
+  frc::AnalogGyro m_gyro{0};
+
+  frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
+
+  frc::DifferentialDrivePoseEstimator m_poseEstimator{
+      m_kinematics, m_gyro.GetRotation2d(),
+      units::meter_t{m_leftEncoder.GetDistance()},
+      units::meter_t{m_rightEncoder.GetDistance()}, frc::Pose2d{}};
+
+  PhotonCameraWrapper m_pcw;
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
+
+  // Simulation classes help us simulate our robot
+  frc::sim::AnalogGyroSim m_gyroSim{m_gyro};
+  frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
+  frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
+  frc::Field2d m_fieldSim;
+  frc::LinearSystem<2, 2, 2> m_drivetrainSystem =
+      frc::LinearSystemId::IdentifyDrivetrainSystem(
+          1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
+  frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
+      m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in};
+};
--- a/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h
+++ b/photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h
@@ -0,0 +1,46 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 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 <photonlib/PhotonCamera.h>
+#include <photonlib/PhotonPoseEstimator.h>
+
+#include <utility>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/apriltag/AprilTagFields.h>
+
+class PhotonCameraWrapper {
+ public:
+  photonlib::PhotonPoseEstimator m_poseEstimator{
+      frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
+      photonlib::LOWEST_AMBIGUITY,
+      std::move(photonlib::PhotonCamera{"WPI2023"}), frc::Transform3d{}};
+
+  inline std::optional<photonlib::EstimatedRobotPose> Update(
+      frc::Pose2d estimatedPose) {
+    m_poseEstimator.SetReferencePose(frc::Pose3d(estimatedPose));
+    return m_poseEstimator.Update();
+  }
+};
--- a/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h
+++ b/photonlib-cpp-examples/apriltagExample/src/main/include/Robot.h
@@ -0,0 +1,50 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 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 <photonlib/PhotonCamera.h>
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/PWMVictorSPX.h>
+#include <units/angle.h>
+#include <units/length.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void TeleopPeriodic() override;
+  void SimulationPeriodic() override;
+
+ private:
+  frc::XboxController m_controller{0};
+
+  Drivetrain m_drive;
+};
--- a/photonlib-cpp-examples/apriltagExample/src/test/cpp/main.cpp
+++ b/photonlib-cpp-examples/apriltagExample/src/test/cpp/main.cpp
@@ -0,0 +1,34 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 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.
+ */
+
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}