Add old nanobind code

PhotonVision · Sep 26, 2024 · 8093db0 · 8093db0
1 parent 8cd79df
commit 8093db0
Show file tree

Hide file tree

Showing 15 changed files with 88 additions and 82 deletions.
diff --git a/out.txt b/out.txt
diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle
@@ -111,8 +111,13 @@ model {
                 lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared'
                 lib library: nativeName
 
-                nativeUtils.useRequiredLibrary(it, "wpilib_shared")
             }
+
+            nativeUtils.useRequiredLibrary(it, "wpilib_shared")
+            nativeUtils.useRequiredLibrary(it, "cscore_shared")
+            nativeUtils.useRequiredLibrary(it, "cameraserver_shared")
+            nativeUtils.useRequiredLibrary(it, "apriltag_shared")
+            nativeUtils.useRequiredLibrary(it, "opencv_shared")
         }
     }
     testSuites {
@@ -185,7 +190,7 @@ model {
 
     binaries {
         withType(NativeBinarySpec).all {
-            println "${it.toolChain} -> ${it.component.baseName} -> ${it.toolChain instanceof VisualCpp}"
+            // println "${it.toolChain} -> ${it.component.baseName} -> ${it.toolChain instanceof VisualCpp}"
 
             if((it.component.baseName == "photonlibpy" || it.component.baseName == nativeName)) {
                 if (it.toolChain instanceof VisualCpp) {
@@ -267,7 +272,6 @@ task installPhotonlibpyNative(type: Copy) {
                 "opencv_imgcodecs480.dll",
                 "opencv_flann480.dll",
                 "opencv_imgproc480.dll",
-
                 "libcameraserver.so",
                 "libcscore.so",
                 "libopencv_core.so.4.8",

diff --git a/photon-lib/clang-format.sh b/photon-lib/clang-format.sh
diff --git a/photon-lib/py/.gitignore b/photon-lib/py/.gitignore
@@ -6,3 +6,4 @@ photonlibpy/version.py
 photonlibpy/*.so*
 photonlibpy/*.dylib*
 photonlibpy/*.dll*
+photonlibpy/*.pyd
diff --git a/photon-lib/py/create_photonlib_pyi.py b/photon-lib/py/create_photonlib_pyi.py
@@ -1,7 +1,7 @@
 def write_stubgen():
     import os
 
-    cwd = os.getcwd()
+    os.getcwd()
 
     # From nanobind==2.1.0
     from nanobind.stubgen import StubGen

diff --git a/photon-lib/py/photonlibpy/__init__.py b/photon-lib/py/photonlibpy/__init__.py
@@ -32,5 +32,6 @@
 
 # and now our extension module
 import os
+
 os.add_dll_directory(os.path.dirname(os.path.realpath(__file__)))
 from ._photonlibpy import *
diff --git a/photon-lib/py/setup.py b/photon-lib/py/setup.py
@@ -1,5 +1,4 @@
 import os
-import platform
 from setuptools import setup, find_packages
 import subprocess, re
 

diff --git a/photon-lib/src/main/pybindings/cpp/photonlib_nanobind.cpp b/photon-lib/src/main/pybindings/cpp/photonlib_nanobind.cpp
@@ -23,19 +23,50 @@
  */
 
 #include <fmt/format.h>
+
+#include <memory>
+
 #include "photon/PhotonCamera.h"
+#include "photon/simulation/VisionSystemSim.h"
 
 // actual nanobind include
 #include <nanobind/nanobind.h>
 #include <nanobind/stl/string.h>
 #include <nanobind/stl/vector.h>
 #include <nanobind/stl/optional.h>
 
-NB_MODULE(_photonlibpy, m) {
-  namespace nb = nanobind;
+namespace nb = nanobind;
+using namespace nb::literals;
 
-  m.doc() = "C++ bindings for photonlib";
+// repr helper
+template <> struct fmt::formatter<photon::PhotonTrackedTarget> : formatter<string_view> {
+  auto format(photon::PhotonTrackedTarget const &c, format_context &ctx) const {
+    return fmt::format_to(ctx.out(), "PhotonTrackedTarget<yaw={},pitch={}>", c.yaw, c.pitch);
+  }
+};
+template <> struct fmt::formatter<photon::PhotonPipelineMetadata> : formatter<string_view> {
+  auto format(photon::PhotonPipelineMetadata const &c, format_context &ctx) const {
+    return fmt::format_to(ctx.out(), "PhotonPipelineMetadata<sequenceID={}>", c.sequenceID);
+  }
+};
+
+frc::Pose3d makePose(double x, double y, double z, double W, double X, double Y,
+                     double Z) {
+  return frc::Pose3d{frc::Translation3d{units::meter_t{x}, units::meter_t{y},
+                                        units::meter_t{z}},
+                     frc::Rotation3d{frc::Quaternion{W, X, Y, Z}}};
+}
 
+void wrap_geom(nb::module_ m) {
+  using namespace frc;
+  nb::class_<Transform3d>(m, "Transform3d").def(nb::init<>());
+  nb::class_<Pose3d>(m, "Pose3d")
+      .def(nb::init<>())
+      .def(nb::new_(&makePose),
+           "Create a Pose3d from translation/rotation components");
+}
+
+void wrap_photon(nb::module_ m) {
   nb::class_<photon::PhotonPipelineMetadata>(m, "PhotonPipelineMetadata")
       .def_ro("sequenceID", &photon::PhotonPipelineMetadata::sequenceID)
       .def_ro("captureTimestampMicros",
@@ -50,20 +81,56 @@ NB_MODULE(_photonlibpy, m) {
       .def("__repr__", [](const photon::PhotonTrackedTarget& t) {
         std::string s;
         fmt::format_to(std::back_inserter(s),
-                       "PhotonTrackedTarget<yaw={},pitch={}>", t.yaw, t.pitch);
+                       "{}>", t);
         return s;
       });
   nb::class_<photon::MultiTargetPNPResult>(m, "MultiTargetPNPResult")
-      .def_ro("fiducialIDsUsed", &photon::MultiTargetPNPResult::fiducialIDsUsed)
-  ;
+      .def_ro("fiducialIDsUsed",
+              &photon::MultiTargetPNPResult::fiducialIDsUsed);
 
   nb::class_<photon::PhotonPipelineResult>(m, "PhotonPipelineResult")
       .def_ro("metadata", &photon::PhotonPipelineResult::metadata)
       .def_ro("targets", &photon::PhotonPipelineResult::targets)
-      .def_ro("multitagResult", &photon::PhotonPipelineResult::multitagResult);
+      .def_ro("multitagResult", &photon::PhotonPipelineResult::multitagResult)
+      .def("__repr__", [](const photon::PhotonPipelineResult& t) {
+        std::string s;
+        fmt::format_to(std::back_inserter(s),
+                       "PhotonPipelineResult<metadata={},targets=[{}]>", t.metadata, fmt::join(t.targets, ", "));
+        return s;
+      });
 
   nb::class_<photon::PhotonCamera>(m, "PhotonCamera")
       .def(nb::init<const std::string&>())
       .def("GetDriverMode", &photon::PhotonCamera::GetDriverMode)
       .def("GetLatestResult", &photon::PhotonCamera::GetLatestResult);
 }
+
+void wrap_photon_sim(nb::module_ m) {
+  using namespace photon;
+
+  nb::class_<VisionSystemSim>(m, "VisionSystemSim")
+      .def(nb::init<const std::string&>())
+      .def("AddCamera", &VisionSystemSim::AddCamera)
+      .def("AddVisionTargets",
+           nb::overload_cast<std::string, const std::vector<VisionTargetSim>&>(
+               &VisionSystemSim::AddVisionTargets))
+      .def("Update",
+           nb::overload_cast<const frc::Pose3d&>(&VisionSystemSim::Update));
+
+  nb::class_<PhotonCameraSim>(m, "PhotonCameraSim")
+      .def(nb::init<PhotonCamera*>());
+
+  nb::class_<VisionTargetSim>(m, "VisionTargetSim")
+      .def(nb::init<frc::Pose3d, TargetModel>(), "pose"_a, "model"_a, "Create a simulated target at a given pose")
+      .def(nb::init<frc::Pose3d, TargetModel, int>(), "pose"_a, "model"_a, "fiducial_id"_a, "Create a simulated AprilTag at a given pose");
+
+  nb::class_<TargetModel>(m, "TargetModel").def(nb::init<units::meter_t, units::meter_t>());
+}
+
+NB_MODULE(_photonlibpy, m) {
+  m.doc() = "C++ bindings for photonlib";
+
+  wrap_photon(m);
+  wrap_photon_sim(m);
+  wrap_geom(m);
+}
diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp
@@ -17,6 +17,8 @@
 
 #include "photon/targeting/proto/MultiTargetPNPResultProto.h"
 
+#include <vector>
+
 #include "photon.pb.h"
 #include "photon/targeting/proto/PNPResultProto.h"
 

diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
@@ -48,6 +48,7 @@ namespace VisionEstimation {
 
 #include <iostream>
 
+[[maybe_unused]]
 static std::optional<PnpResult> EstimateCamPosePNP(
     const Eigen::Matrix<double, 3, 3>& cameraMatrix,
     const Eigen::Matrix<double, 8, 1>& distCoeffs,

diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp
@@ -16,6 +16,7 @@
  */
 
 #include <chrono>
+#include <vector>
 
 #include <units/angle.h>
 

diff --git a/photonlib-python-examples/aimandrange/robot.py b/photonlib-python-examples/aimandrange/robot.py
@@ -25,8 +25,6 @@
 
 import math
 import wpilib
-import wpimath
-import wpimath.geometry
 import drivetrain
 
 from photonlibpy import PhotonCamera

diff --git a/photonlib-python-examples/aimandrange/tests/pyfrc_test.py b/photonlib-python-examples/aimandrange/tests/pyfrc_test.py
@@ -2,5 +2,3 @@
     This test module imports tests that come with pyfrc, and can be used
     to test basic functionality of just about any robot.
 """
-
-from pyfrc.tests import *
diff --git a/photonlib-python-examples/aimattarget/tests/pyfrc_test.py b/photonlib-python-examples/aimattarget/tests/pyfrc_test.py
@@ -2,5 +2,3 @@
     This test module imports tests that come with pyfrc, and can be used
     to test basic functionality of just about any robot.
 """
-
-from pyfrc.tests import *
diff --git a/photonlib-python-examples/poseest/tests/pyfrc_test.py b/photonlib-python-examples/poseest/tests/pyfrc_test.py
@@ -2,5 +2,3 @@
     This test module imports tests that come with pyfrc, and can be used
     to test basic functionality of just about any robot.
 """
-
-from pyfrc.tests import *