From 1724de07c3a9848c06d1ba19ff2a79cf5e1d5cfb Mon Sep 17 00:00:00 2001 From: Alex Pereira Date: Sun, 26 Feb 2023 12:04:51 -0500 Subject: [PATCH] Fixed issues with using class methods Signed-off-by: Alex Pereira --- README.md | 5 +- frc_apriltags/Utilities/AprilTag.py | 61 ++++++++----------- .../Utilities/AprilTagFieldLayout.py | 23 +++---- frc_apriltags/apriltags.py | 5 +- frc_apriltags/camera.py | 2 +- pyproject.toml | 6 +- tests/test.py | 30 +++++++++ 7 files changed, 75 insertions(+), 57 deletions(-) create mode 100644 tests/test.py diff --git a/README.md b/README.md index c8fea00..a6081b6 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,6 @@
  • The OpenCV Contributor package
  • The NumPy package (should install with pupil-apriltags)
  • The RobotPy package
    • -
  • The Transforms3d package
  • The Pupil Apriltags package

    • @@ -28,14 +27,14 @@

    - pip install -U pip wheel setuptools transforms3d opencv-contrib-python pupil-apriltags robotpy[cscore] + pip install -U pip wheel setuptools opencv-contrib-python pupil-apriltags robotpy[cscore]

    How to install on Windows:

    Install Python 3.10.8 for Windows, being sure to add Python to PATH. Then, install the required packages using this command: - pip install -U pip wheel setuptools transforms3d opencv-contrib-python pupil-apriltags robotpy[cscore] + pip install -U pip wheel setuptools opencv-contrib-python pupil-apriltags robotpy[cscore]

    Writing Programs for Jetson

    diff --git a/frc_apriltags/Utilities/AprilTag.py b/frc_apriltags/Utilities/AprilTag.py index 800c485..688cb2f 100644 --- a/frc_apriltags/Utilities/AprilTag.py +++ b/frc_apriltags/Utilities/AprilTag.py @@ -1,23 +1,12 @@ # Import Libraries -import typing -from wpimath.geometry import * +from wpimath.geometry import * # Start of the AprilTag class class AprilTag: """ Use this class to generate an object with the properties of an AprilTag. """ - @typing.overload - def __init__(self) -> None: - """ - Constructor for the AprilTag class. - """ - # Creates a default tag - self.id = 0 - self.pose = Pose3d() - - @typing.overload - def __init__(self, id: int, pose: Pose3d) -> None: + def __init__(self, id: int = 0, pose: Pose3d = Pose3d()): """ Constructor for the AprilTag class. @param tagId @@ -27,20 +16,22 @@ def __init__(self, id: int, pose: Pose3d) -> None: self.id = id self.pose = pose - @typing.overload - def __init__(self, id: int, trans: Translation3d, rot: Rotation3d) -> None: + @classmethod + def fromPoseComponents(cls, id: int, trans: Translation3d, rot: Rotation3d): """ Constructor for the AprilTag class. @param tagId @param Translation3d @param Rotation3d """ - # Localize parameters - self.id = id - self.pose = Pose3d(trans, rot) + # Generate 3D parts + pose = Pose3d(trans, rot) - @typing.overload - def __init__(self, id: int, x: float, y: float, z: float, rMatrix) -> None: + # Return the class + return AprilTag(id, pose) + + @classmethod + def fromMatrix(cls, id: int, x: float, y: float, z: float, rMatrix): """ Constructor for the AprilTag class. @param tagId @@ -52,31 +43,31 @@ def __init__(self, id: int, x: float, y: float, z: float, rMatrix) -> None: # Generate 3D parts rot = Rotation3d(rMatrix) trans = Translation3d(x, y, z) + pose = Pose3d(trans, rot) - # Localize parameters - self.id = id - self.pose = Pose3d(trans, rot) + # Return the class + return AprilTag(id, pose) - @typing.overload - def __init__(self, id: int, x: float, y: float, z: float, quaterion: Quaternion) -> None: + @classmethod + def fromQuaternion(cls, id: int, x: float, y: float, z: float, q: Quaternion): """ Constructor for the AprilTag class. @param tagId @param x: The x offset in meters @param y: The y offset in meters @param z: The z offset in meters - @param quaterion: The 4x4 Wuaterion + @param q: The 4x4 Quaterion """ # Generate 3D parts - rot = Rotation3d(quaterion) + rot = Rotation3d(q) trans = Translation3d(x, y, z) + pose = Pose3d(trans, rot) - # Localize parameters - self.id = id - self.pose = Pose3d(trans, rot) + # Return the class + return AprilTag(id, pose) - @typing.overload - def __init__(self, id: int, x: float, y: float, z: float, roll: float, pitch: float, yaw: float) -> None: + @classmethod + def fromDetailed(cls, id: int, x: float, y: float, z: float, roll: float, pitch: float, yaw: float): """ Constructor for the AprilTag class. @param tagId @@ -90,10 +81,10 @@ def __init__(self, id: int, x: float, y: float, z: float, roll: float, pitch: fl # Generate 3D parts rot = Rotation3d(roll, pitch, yaw) trans = Translation3d(x, y, z) + pose = Pose3d(trans, rot) - # Localize parameters - self.id = id - self.pose = Pose3d(trans, rot) + # Return the class + return AprilTag(id, pose) def getId(self) -> int: """ diff --git a/frc_apriltags/Utilities/AprilTagFieldLayout.py b/frc_apriltags/Utilities/AprilTagFieldLayout.py index 3b4e01c..a2ec1ba 100644 --- a/frc_apriltags/Utilities/AprilTagFieldLayout.py +++ b/frc_apriltags/Utilities/AprilTagFieldLayout.py @@ -1,6 +1,5 @@ # Import Libraries import json -import typing import numpy as np from enum import Enum from typing import Sequence @@ -12,13 +11,12 @@ from frc_apriltags.Utilities.AprilTag import AprilTag # Creates the AprilTagFieldLayout class -class AprilTagFieldLayout: +class AprilTagFieldLayout: class Origin(Enum): kBlueAllianceWallRightSide = "BlueWall" kRedAllianceWallRightSide = "RedWall" - @typing.overload - def __init__(self, tags: Sequence[AprilTag], fieldLength: float, fieldWidth: float, isRed = False) -> None: + def __init__(self, tags: Sequence[AprilTag], fieldLength: float, fieldWidth: float, isRed = False): """ Generates an field with AprilTags for testing. @param allTags: A list of all known tags @@ -59,15 +57,15 @@ def __init__(self, tags: Sequence[AprilTag], fieldLength: float, fieldWidth: flo elif (isRed == False): self.setOrigin(AprilTagFieldLayout.Origin.kBlueAllianceWallRightSide) - @typing.overload - def __init__(self, name: str, isRed: bool) -> None: + @classmethod + def fromJson(cls, name: str, isRed: bool): """ Generates an field with AprilTags from the WPILib json file. @param name: The name of the json file in the year-gamename format @param isRed: If you are on the red alliance """ # Creates a blank array - self.allTags = [Pose3d()] * 9 + allTags = [AprilTag()] * 9 # Returns the JSON as a dictionary with resources.open_binary("frc_apriltags.Tag_Layouts", name + ".json") as fp: @@ -93,14 +91,17 @@ def __init__(self, name: str, isRed: bool) -> None: q = Quaternion(w_rot, x_rot, y_rot, z_rot) # Creates a Pose3d object - pose = Pose3d(x_trans, y_trans, z_trans, Rotation3d(q)) + tag = AprilTag.fromQuaternion(id, x_trans, y_trans, z_trans, q) # Adds the tag to the allTags array - self.allTags[id] = pose + allTags[id] = tag # Sets the field dimensions - self.fieldLength = data["field"]["length"] - self.fieldWidth = data["field"]["width"] + fieldLength = data["field"]["length"] + fieldWidth = data["field"]["width"] + + # Return the class + return cls(allTags, fieldLength, fieldWidth, isRed) def setOrigin(self, origin): """ diff --git a/frc_apriltags/apriltags.py b/frc_apriltags/apriltags.py index bf733af..3f13e42 100644 --- a/frc_apriltags/apriltags.py +++ b/frc_apriltags/apriltags.py @@ -2,14 +2,12 @@ import cv2 as cv import numpy as np import pupil_apriltags -from wpilib import Timer -from wpimath.geometry import Pose3d, Rotation3d, Translation3d # Import Classes from frc_apriltags.communications import NetworkCommunications # Import Utilities -from frc_apriltags.Utilities.Units import Units +from frc_apriltags.Utilities.Units import Units from frc_apriltags.Utilities.Logger import Logger # The size of the tag in meters @@ -25,7 +23,6 @@ def __init__(self) -> None: Constructor for the Detector class. """ # Instance creation - self.timer = Timer() self.comms = NetworkCommunications() # Creates a pupil apriltags detector diff --git a/frc_apriltags/camera.py b/frc_apriltags/camera.py index 3db2e34..a119f5c 100644 --- a/frc_apriltags/camera.py +++ b/frc_apriltags/camera.py @@ -1,5 +1,5 @@ # Import Libraries -import cv2 as cv +import cv2 as cv import numpy as np # Import Classes diff --git a/pyproject.toml b/pyproject.toml index edae825..58100ee 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -17,7 +17,7 @@ name = "frc-apriltags" # Required # # For a discussion on single-sourcing the version, see # https://packaging.python.org/guides/single-sourcing-package-version/ -version = "0.4.1.b0" # Required +version = "0.5.0" # Required # Pre-releases denotations: # dev releases (denoted with a “.devN” suffix) # alpha releases (denoted with a “.aN” suffix) @@ -43,7 +43,7 @@ readme = "README.md" # Optional # 'Programming Language' classifiers above, 'pip install' will check this # and refuse to install the project if the version does not match. See # https://packaging.python.org/guides/distributing-packages-using-setuptools/#python-requires -requires-python = ">=3.8,<3.12" +requires-python = ">=3.7,<3.12" # This is either text indicating the license for the distribution, or a file # that contains the license @@ -56,7 +56,7 @@ license = {file = "LICENSE.txt"} # Note that this is a list of additional keywords, separated # by commas, to be used to assist searching for the distribution in a # larger catalog. -keywords = ["FRC", "AprilTags", "OpenCV"] # Optional +keywords = ["FRC-AprilTags", "FRC", "AprilTags", "OpenCV"] # Optional # This should be your name or the name of the organization who originally # authored the project, and a valid email address corresponding to the name diff --git a/tests/test.py b/tests/test.py new file mode 100644 index 0000000..9e7e3c0 --- /dev/null +++ b/tests/test.py @@ -0,0 +1,30 @@ +import numpy as np +from wpimath.geometry import * +from frc_apriltags.Utilities.AprilTagFieldLayout import AprilTagFieldLayout + +field = AprilTagFieldLayout.fromJson("2023-chargedup", False) +print(field.getTagPose(7)) + +pose1 = Pose3d( + Translation3d(0, 0, 0), + Rotation3d(0, 0, np.pi*0) +) + +# Gets things in the right direction +pose2 = Pose3d( + Translation3d(1, 1, 0), + Rotation3d(0, 0, 0) +) +trans1 = Transform3d( + Translation3d(), + Rotation3d(pose1.rotation().X(), pose1.rotation().Y(), pose1.rotation().Z()) +) +pose2 = pose2.transformBy(trans1) + +# +trans2 = Transform3d( + pose2.translation(), + pose2.rotation() +) + +print(pose1.transformBy(trans2).toPose2d()) \ No newline at end of file