Yeet PhotonCamera from PhotonPoseEstimator

Author: auscompgeek

gen/RobotPoseEstimator.yml

@@ -4,6 +4,7 @@ extra_includes:
 
 enums:
   PoseStrategy:
+    ignore: true  # duplicated in PhotonPoseEstimator.h
 classes:
   RobotPoseEstimator:
     force_type_casters:

photonvision/src/headers.patch

@@ -0,0 +1,89 @@
+From 16749a065bb5bad6c62764d78a1a2ae1ca4ee6f7 Mon Sep 17 00:00:00 2001
+From: David Vo <[email protected]>
+Date: Sat, 11 Feb 2023 17:50:54 +1100
+Subject: [PATCH] Yeet PhotonCamera from PhotonPoseEstimator
+
+---
+ photonlib/PhotonPoseEstimator.h | 16 +---------------
+ 1 file changed, 1 insertion(+), 15 deletions(-)
+
+diff --git a/photonlib/PhotonPoseEstimator.h b/photonlib/PhotonPoseEstimator.h
+index 10b89f499c0a..5ed0ee53f11d 100644
+--- a/photonlib/PhotonPoseEstimator.h
++++ b/photonlib/PhotonPoseEstimator.h
+@@ -31,12 +31,10 @@
+ 
+ #include <frc/apriltag/AprilTagFieldLayout.h>
+ #include <frc/geometry/Pose3d.h>
+ #include <frc/geometry/Transform3d.h>
+ 
+-#include "photonlib/PhotonCamera.h"
+-
+ namespace photonlib {
+ enum PoseStrategy : int {
+   LOWEST_AMBIGUITY,
+   CLOSEST_TO_CAMERA_HEIGHT,
+   CLOSEST_TO_REFERENCE_POSE,
+@@ -61,12 +59,10 @@ struct EstimatedRobotPose {
+  * in field pose, using the strategy set below. Example usage can be found in
+  * our apriltagExample example project.
+  */
+ class PhotonPoseEstimator {
+  public:
+-  using map_value_type =
+-      std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
+   using size_type = std::vector<map_value_type>::size_type;
+ 
+   /**
+    * Create a new PhotonPoseEstimator.
+    *
+@@ -75,16 +71,15 @@ class PhotonPoseEstimator {
+    * at (1.0,2.0,3.0) </code> }
+    *
+    * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
+    * respect to the FIRST field.
+    * @param strategy The strategy it should use to determine the best pose.
+-   * @param camera PhotonCameras and
+    * @param robotToCamera Transform3d from the center of the robot to the camera
+    * mount positions (ie, robot ➔ camera).
+    */
+   explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
+-                               PoseStrategy strategy, PhotonCamera&& camera,
++                               PoseStrategy strategy,
+                                frc::Transform3d robotToCamera);
+ 
+   /**
+    * Get the AprilTagFieldLayout being used by the PositionEstimator.
+    *
+@@ -147,28 +142,19 @@ class PhotonPoseEstimator {
+    *
+    * @param lastPose the lastPose to set
+    */
+   inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
+ 
+-  /**
+-   * Update the pose estimator. Internally grabs a new PhotonPipelineResult from
+-   * the camera and process it.
+-   */
+-  std::optional<EstimatedRobotPose> Update();
+-
+   /**
+    * Update the pose estimator.
+    */
+   std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result);
+ 
+-  inline PhotonCamera& GetCamera() { return camera; }
+-
+  private:
+   frc::AprilTagFieldLayout aprilTags;
+   PoseStrategy strategy;
+ 
+-  PhotonCamera camera;
+   frc::Transform3d m_robotToCamera;
+ 
+   frc::Pose3d lastPose;
+   frc::Pose3d referencePose;
+ 
+-- 
+2.39.1
+

photonvision/src/sources.patch

@@ -0,0 +1,48 @@
+From 16749a065bb5bad6c62764d78a1a2ae1ca4ee6f7 Mon Sep 17 00:00:00 2001
+From: David Vo <[email protected]>
+Date: Sat, 11 Feb 2023 17:50:54 +1100
+Subject: [PATCH] Yeet PhotonCamera from PhotonPoseEstimator
+
+---
+ photonlib/PhotonPoseEstimator.cpp | 9 +--------
+ 1 file changed, 1 insertion(+), 8 deletions(-)
+
+diff --git a/photonlib/PhotonPoseEstimator.cpp b/photonlib/PhotonPoseEstimator.cpp
+index ecdc03eb1b80..81e474c63a63 100644
+--- a/photonlib/PhotonPoseEstimator.cpp
++++ b/photonlib/PhotonPoseEstimator.cpp
+@@ -36,30 +36,23 @@
+ #include <frc/geometry/Pose3d.h>
+ #include <frc/geometry/Rotation3d.h>
+ #include <frc/geometry/Transform3d.h>
+ #include <units/time.h>
+ 
+-#include "photonlib/PhotonCamera.h"
+ #include "photonlib/PhotonPipelineResult.h"
+ #include "photonlib/PhotonTrackedTarget.h"
+ 
+ namespace photonlib {
+ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
+-                                         PoseStrategy strat, PhotonCamera&& cam,
++                                         PoseStrategy strat,
+                                          frc::Transform3d robotToCamera)
+     : aprilTags(tags),
+       strategy(strat),
+-      camera(std::move(cam)),
+       m_robotToCamera(robotToCamera),
+       lastPose(frc::Pose3d()),
+       referencePose(frc::Pose3d()) {}
+ 
+-std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
+-  auto result = camera.GetLatestResult();
+-  return Update(result);
+-}
+-
+ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
+     const PhotonPipelineResult& result) {
+   if (!result.HasTargets()) {
+     return std::nullopt;
+   }
+-- 
+2.39.1
+

pyproject.toml

@@ -24,19 +24,31 @@ install_requires = [
 [tool.robotpy-build]
 base_package = "photonvision"
 
-[tool.robotpy-build.static_libs."photonlib".maven_lib_download]
+[tool.robotpy-build.wrappers."photonvision".maven_lib_download]
 artifact_id = "PhotonLib-cpp"
 group_id = "org.photonvision"
 repo_url = "https://maven.photonvision.org/repository/snapshots"
 version = "dev-v2023.3.0-6-g0255798d"
 
-libs = ["Photon"]
+use_sources = true
+sources = [
+    "photonlib/PhotonCamera.cpp",
+    "photonlib/PhotonPipelineResult.cpp",
+    "photonlib/PhotonPoseEstimator.cpp",
+    "photonlib/PhotonTrackedTarget.cpp",
+    "photonlib/RobotPoseEstimator.cpp",
+]
+
+[[tool.robotpy-build.wrappers."photonvision".maven_lib_download.patches]]
+patch = "photonvision/src/sources.patch"
+
+[[tool.robotpy-build.wrappers."photonvision".maven_lib_download.header_patches]]
+patch = "photonvision/src/headers.patch"
 
 [tool.robotpy-build.wrappers."photonvision"]
 name = "photonvision"
 
 depends = [
-    "photonlib",
     "apriltag",
     "wpilibc",
     "wpimath_geometry",
@@ -57,7 +69,7 @@ generation_data = "gen"
 Packet = "photonlib/Packet.h"
 PhotonCamera = "photonlib/PhotonCamera.h"
 PhotonPipelineResult = "photonlib/PhotonPipelineResult.h"
-#PhotonPoseEstimator = "photonlib/PhotonPoseEstimator.h"
+PhotonPoseEstimator = "photonlib/PhotonPoseEstimator.h"
 PhotonTargetSortMode = "photonlib/PhotonTargetSortMode.h"
 PhotonTrackedTarget = "photonlib/PhotonTrackedTarget.h"
 PhotonUtils = "photonlib/PhotonUtils.h"

tests/test_pose_estimator.py

@@ -0,0 +1,15 @@
+import robotpy_apriltag
+from wpimath.geometry import Transform3d
+
+import photonvision
+
+
+def test_photon_pose_estimator_ctor():
+    estimator = photonvision.PhotonPoseEstimator(
+        robotpy_apriltag.loadAprilTagLayoutField(
+            robotpy_apriltag.AprilTagField.k2023ChargedUp
+        ),
+        photonvision.PoseStrategy.AVERAGE_BEST_TARGETS,
+        Transform3d(),
+    )
+    assert estimator