From 47aea29b6b3e0e1e16d1ed59a51e7799c57e51f0 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Sat, 16 Dec 2023 12:32:49 -0600 Subject: [PATCH 1/3] Add photonlibpy (#1040) * Added a pure-python implementation of photonlib, named photonlibpy and hosted on pypi --------- Co-authored-by: Matt --- .github/workflows/python.yml | 62 +++++ photon-lib/py/.gitignore | 5 + photon-lib/py/buildAndTest.bat | 14 + photon-lib/py/photonlibpy/__init__.py | 1 + .../py/photonlibpy/multiTargetPNPResult.py | 45 ++++ photon-lib/py/photonlibpy/packet.py | 143 +++++++++++ photon-lib/py/photonlibpy/photonCamera.py | 170 +++++++++++++ .../py/photonlibpy/photonPipelineResult.py | 38 +++ .../py/photonlibpy/photonTrackedTarget.py | 82 ++++++ photon-lib/py/setup.py | 54 ++++ photon-lib/py/test/data.py | 239 ++++++++++++++++++ photon-lib/py/test/photonlibpy_test.py | 46 ++++ 12 files changed, 899 insertions(+) create mode 100644 .github/workflows/python.yml create mode 100644 photon-lib/py/.gitignore create mode 100644 photon-lib/py/buildAndTest.bat create mode 100644 photon-lib/py/photonlibpy/__init__.py create mode 100644 photon-lib/py/photonlibpy/multiTargetPNPResult.py create mode 100644 photon-lib/py/photonlibpy/packet.py create mode 100644 photon-lib/py/photonlibpy/photonCamera.py create mode 100644 photon-lib/py/photonlibpy/photonPipelineResult.py create mode 100644 photon-lib/py/photonlibpy/photonTrackedTarget.py create mode 100644 photon-lib/py/setup.py create mode 100644 photon-lib/py/test/data.py create mode 100644 photon-lib/py/test/photonlibpy_test.py diff --git a/.github/workflows/python.yml b/.github/workflows/python.yml new file mode 100644 index 0000000000..de54a54756 --- /dev/null +++ b/.github/workflows/python.yml @@ -0,0 +1,62 @@ +name: Build and Distribute PhotonLibPy + +permissions: + id-token: write # IMPORTANT: this permission is mandatory for trusted publishing + +on: + push: + branches: [ master ] + tags: + - 'v*' + pull_request: + branches: [ master ] + +jobs: + buildAndDeploy: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v3 + with: + sparse-checkout-cone-mode: false + fetch-tags: true + fetch-depth: 99999 + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: 3.11 + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install setuptools wheel pytest + + - name: Build wheel + working-directory: ./photon-lib/py + run: | + python setup.py sdist bdist_wheel + + - name: Run Unit Tests + working-directory: ./photon-lib/py + run: | + pip install --no-cache-dir dist/*.whl + pytest + + + - name: Upload artifacts + uses: actions/upload-artifact@master + with: + name: dist + path: ./photon-lib/py/dist/ + + - name: Publish package distributions to TestPyPI + # Only upload on tags + if: startsWith(github.ref, 'refs/tags/v') + uses: pypa/gh-action-pypi-publish@release/v1 + with: + packages_dir: ./photon-lib/py/dist/ + + permissions: + id-token: write # IMPORTANT: this permission is mandatory for trusted publishing diff --git a/photon-lib/py/.gitignore b/photon-lib/py/.gitignore new file mode 100644 index 0000000000..f0c4ced6d6 --- /dev/null +++ b/photon-lib/py/.gitignore @@ -0,0 +1,5 @@ +photonlibpy.egg-info/ +dist/ +build/ +.eggs/ +photonlibpy/version.py diff --git a/photon-lib/py/buildAndTest.bat b/photon-lib/py/buildAndTest.bat new file mode 100644 index 0000000000..b9774a54d6 --- /dev/null +++ b/photon-lib/py/buildAndTest.bat @@ -0,0 +1,14 @@ +:: Uninstall if it already was installed +pip uninstall -y photonlibpy + +:: Build wheel +python setup.py bdist_wheel + +:: Install whatever wheel was made +for %%f in (dist/*.whl) do ( + echo installing dist/%%f + pip install --no-cache-dir dist/%%f +) + +:: Run the test suite +pytest diff --git a/photon-lib/py/photonlibpy/__init__.py b/photon-lib/py/photonlibpy/__init__.py new file mode 100644 index 0000000000..d5b258f5e7 --- /dev/null +++ b/photon-lib/py/photonlibpy/__init__.py @@ -0,0 +1 @@ +# No one here but us chickens diff --git a/photon-lib/py/photonlibpy/multiTargetPNPResult.py b/photon-lib/py/photonlibpy/multiTargetPNPResult.py new file mode 100644 index 0000000000..63bbb34301 --- /dev/null +++ b/photon-lib/py/photonlibpy/multiTargetPNPResult.py @@ -0,0 +1,45 @@ +from dataclasses import dataclass, field +from wpimath.geometry import Transform3d +from photonlibpy.packet import Packet + + +@dataclass +class PNPResult: + _NUM_BYTES_IN_FLOAT = 8 + PACK_SIZE_BYTES = 1 + (_NUM_BYTES_IN_FLOAT * 7 * 2) + (_NUM_BYTES_IN_FLOAT * 3) + + isPresent: bool = False + best: Transform3d = field(default_factory=Transform3d) + alt: Transform3d = field(default_factory=Transform3d) + ambiguity: float = 0.0 + bestReprojError: float = 0.0 + altReprojError: float = 0.0 + + def createFromPacket(self, packet: Packet) -> Packet: + self.isPresent = packet.decodeBoolean() + self.best = packet.decodeTransform() + self.alt = packet.decodeTransform() + self.bestReprojError = packet.decodeDouble() + self.altReprojError = packet.decodeDouble() + self.ambiguity = packet.decodeDouble() + return packet + + +@dataclass +class MultiTargetPNPResult: + _MAX_IDS = 32 + # pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally) + _PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (1 * _MAX_IDS) + + estimatedPose: PNPResult = field(default_factory=PNPResult) + fiducialIDsUsed: list[int] = field(default_factory=list) + + def createFromPacket(self, packet: Packet) -> Packet: + self.estimatedPose = PNPResult() + self.estimatedPose.createFromPacket(packet) + self.fiducialIDsUsed = [] + for _ in range(MultiTargetPNPResult._MAX_IDS): + fidId = packet.decode16() + if fidId >= 0: + self.fiducialIDsUsed.append(fidId) + return packet diff --git a/photon-lib/py/photonlibpy/packet.py b/photon-lib/py/photonlibpy/packet.py new file mode 100644 index 0000000000..c3816d94f8 --- /dev/null +++ b/photon-lib/py/photonlibpy/packet.py @@ -0,0 +1,143 @@ +import struct +from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Quaternion +import wpilib + + +class Packet: + def __init__(self, data: list[int]): + """ + * Constructs an empty packet. + * + * @param self.size The self.size of the packet buffer. + """ + self.packetData = data + self.size = len(data) + self.readPos = 0 + self.outOfBytes = False + + def clear(self): + """Clears the packet and resets the read and write positions.""" + self.packetData = [0] * self.size + self.readPos = 0 + self.outOfBytes = False + + def getSize(self): + return self.size + + _NO_MORE_BYTES_MESSAGE = """ + Photonlib - Ran out of bytes while decoding. + Make sure the version of photonvision on the coprocessor + matches the version of photonlib running in the robot code. + """ + + def _getNextByte(self) -> int: + retVal = 0x00 + + if not self.outOfBytes: + try: + retVal = 0x00FF & self.packetData[self.readPos] + self.readPos += 1 + except IndexError: + wpilib.reportError(Packet._NO_MORE_BYTES_MESSAGE, True) + self.outOfBytes = True + + return retVal + + def getData(self) -> list[int]: + """ + * Returns the packet data. + * + * @return The packet data. + """ + return self.packetData + + def setData(self, data: list[int]): + """ + * Sets the packet data. + * + * @param data The packet data. + """ + self.clear() + self.packetData = data + self.size = len(self.packetData) + + def _decodeGeneric(self, unpackFormat, numBytes): + # Read ints in from the data buffer + intList = [] + for _ in range(numBytes): + intList.append(self._getNextByte()) + + # Interpret the bytes as a floating point number + value = struct.unpack(unpackFormat, bytes(intList))[0] + + return value + + def decode8(self) -> int: + """ + * Returns a single decoded byte from the packet. + * + * @return A decoded byte from the packet. + """ + return self._decodeGeneric(">b", 1) + + def decode16(self) -> int: + """ + * Returns a single decoded byte from the packet. + * + * @return A decoded byte from the packet. + """ + return self._decodeGeneric(">h", 2) + + def decode32(self) -> int: + """ + * Returns a decoded int (32 bytes) from the packet. + * + * @return A decoded int from the packet. + """ + return self._decodeGeneric(">l", 4) + + def decodeDouble(self) -> float: + """ + * Returns a decoded double from the packet. + * + * @return A decoded double from the packet. + """ + return self._decodeGeneric(">d", 8) + + def decodeBoolean(self) -> bool: + """ + * Returns a decoded boolean from the packet. + * + * @return A decoded boolean from the packet. + """ + return self.decode8() == 1 + + def decodeDoubleArray(self, length: int) -> list[float]: + """ + * Returns a decoded array of floats from the packet. + * + * @return A decoded array of floats from the packet. + """ + ret = [] + for _ in range(length): + ret.append(self.decodeDouble()) + return ret + + def decodeTransform(self) -> Transform3d: + """ + * Returns a decoded Transform3d + * + * @return A decoded Tansform3d from the packet. + """ + x = self.decodeDouble() + y = self.decodeDouble() + z = self.decodeDouble() + translation = Translation3d(x, y, z) + + w = self.decodeDouble() + x = self.decodeDouble() + y = self.decodeDouble() + z = self.decodeDouble() + rotation = Rotation3d(Quaternion(w, x, y, z)) + + return Transform3d(translation, rotation) diff --git a/photon-lib/py/photonlibpy/photonCamera.py b/photon-lib/py/photonlibpy/photonCamera.py new file mode 100644 index 0000000000..0d474a2434 --- /dev/null +++ b/photon-lib/py/photonlibpy/photonCamera.py @@ -0,0 +1,170 @@ +from enum import Enum +import ntcore +from wpilib import Timer +import wpilib +from photonlibpy.packet import Packet +from photonlibpy.photonPipelineResult import PhotonPipelineResult +from photonlibpy.version import PHOTONVISION_VERSION, PHOTONLIB_VERSION + + +class VisionLEDMode(Enum): + kDefault = -1 + kOff = 0 + kOn = 1 + kBlink = 2 + + +lastVersionTimeCheck = 0.0 +_VERSION_CHECK_ENABLED = True + + +def setVersionCheckEnabled(enabled: bool): + _VERSION_CHECK_ENABLED = enabled + + +class PhotonCamera: + def __init__(self, cameraName: str): + instance = ntcore.NetworkTableInstance.getDefault() + self.name = cameraName + self._tableName = "photonvision" + photonvision_root_table = instance.getTable(self._tableName) + self.cameraTable = photonvision_root_table.getSubTable(cameraName) + self.path = self.cameraTable.getPath() + self.rawBytesEntry = self.cameraTable.getRawTopic("rawBytes").subscribe( + "rawBytes", bytes([]), ntcore.PubSubOptions(periodic=0.01, sendAll=True) + ) + + self.driverModePublisher = self.cameraTable.getBooleanTopic( + "driverModeRequest" + ).publish() + self.driverModeSubscriber = self.cameraTable.getBooleanTopic( + "driverMode" + ).subscribe(False) + self.inputSaveImgEntry = self.cameraTable.getIntegerTopic( + "inputSaveImgCmd" + ).getEntry(0) + self.outputSaveImgEntry = self.cameraTable.getIntegerTopic( + "outputSaveImgCmd" + ).getEntry(0) + self.pipelineIndexRequest = self.cameraTable.getIntegerTopic( + "pipelineIndexRequest" + ).publish() + self.pipelineIndexState = self.cameraTable.getIntegerTopic( + "pipelineIndexState" + ).subscribe(0) + self.heartbeatEntry = self.cameraTable.getIntegerTopic("heartbeat").subscribe( + -1 + ) + + self.ledModeRequest = photonvision_root_table.getIntegerTopic( + "ledModeRequest" + ).publish() + self.ledModeState = photonvision_root_table.getIntegerTopic( + "ledModeState" + ).subscribe(-1) + self.versionEntry = photonvision_root_table.getStringTopic("version").subscribe( + "" + ) + + # Existing is enough to make this multisubscriber do its thing + self.topicNameSubscriber = ntcore.MultiSubscriber( + instance, ["/photonvision/"], ntcore.PubSubOptions(topicsOnly=True) + ) + + self.prevHeartbeat = 0 + self.prevHeartbeatChangeTime = Timer.getFPGATimestamp() + + def getLatestResult(self) -> PhotonPipelineResult: + self._versionCheck() + + retVal = PhotonPipelineResult() + packetWithTimestamp = self.rawBytesEntry.getAtomic() + byteList = packetWithTimestamp.value + timestamp = packetWithTimestamp.time + + if len(byteList) < 1: + return retVal + else: + retVal.populateFromPacket(Packet(byteList)) + # NT4 allows us to correct the timestamp based on when the message was sent + retVal.setTimestampSeconds( + timestamp / 1e-6 - retVal.getLatencyMillis() / 1e-3 + ) + return retVal + + def getDriverMode(self) -> bool: + return self.driverModeSubscriber.get() + + def setDriverMode(self, driverMode: bool) -> None: + self.driverModePublisher.set(driverMode) + + def takeInputSnapshot(self) -> None: + self.inputSaveImgEntry.set(self.inputSaveImgEntry.get() + 1) + + def takeOutputSnapshot(self) -> None: + self.outputSaveImgEntry.set(self.outputSaveImgEntry.get() + 1) + + def getPipelineIndex(self) -> int: + return self.pipelineIndexState.get(0) + + def setPipelineIndex(self, index: int) -> None: + self.pipelineIndexRequest.set(index) + + def getLEDMode(self) -> VisionLEDMode: + mode = self.ledModeState.get() + return VisionLEDMode(mode) + + def setLEDMode(self, led: VisionLEDMode) -> None: + self.ledModeRequest.set(led.value) + + def getName(self) -> str: + return self.name + + def isConnected(self) -> bool: + curHeartbeat = self.heartbeatEntry.get() + now = Timer.getFPGATimestamp() + + if curHeartbeat != self.prevHeartbeat: + self.prevHeartbeat = curHeartbeat + self.prevHeartbeatChangeTime = now + + return (now - self.prevHeartbeatChangeTime) < 0.5 + + def _versionCheck(self) -> None: + if not _VERSION_CHECK_ENABLED: + return + + if (Timer.getFPGATimestamp() - lastVersionTimeCheck) < 5.0: + return + + if not self.heartbeatEntry.exists(): + cameraNames = ( + self.cameraTable.getInstance().getTable(self._tableName).getSubTables() + ) + if len(cameraNames) == 0: + wpilib.reportError( + "Could not find any PhotonVision coprocessors on NetworkTables. Double check that PhotonVision is running, and that your camera is connected!", + False, + ) + else: + wpilib.reportError( + f"PhotonVision coprocessor at path {self.path} not found in Network Tables. Double check that your camera names match! Only the following camera names were found: { ''.join(cameraNames)}", + True, + ) + + elif not self.isConnected(): + wpilib.reportWarning( + f"PhotonVision coprocessor at path {self.path} is not sending new data.", + True, + ) + + versionString = self.versionEntry.get(defaultValue="") + if len(versionString) > 0 and versionString != PHOTONVISION_VERSION: + wpilib.reportWarning( + "Photon version " + + PHOTONVISION_VERSION + + " does not match coprocessor version " + + versionString + + f"! Please install photonlibpy version {PHOTONLIB_VERSION}", + True, + ) diff --git a/photon-lib/py/photonlibpy/photonPipelineResult.py b/photon-lib/py/photonlibpy/photonPipelineResult.py new file mode 100644 index 0000000000..3fe02c9fea --- /dev/null +++ b/photon-lib/py/photonlibpy/photonPipelineResult.py @@ -0,0 +1,38 @@ +from dataclasses import dataclass, field + +from photonlibpy.multiTargetPNPResult import MultiTargetPNPResult +from photonlibpy.packet import Packet +from photonlibpy.photonTrackedTarget import PhotonTrackedTarget + + +@dataclass +class PhotonPipelineResult: + latencyMillis: float = -1.0 + timestampSec: float = -1.0 + targets: list[PhotonTrackedTarget] = field(default_factory=list) + multiTagResult: MultiTargetPNPResult = field(default_factory=MultiTargetPNPResult) + + def populateFromPacket(self, packet: Packet) -> Packet: + self.targets = [] + self.latencyMillis = packet.decodeDouble() + self.multiTagResult = MultiTargetPNPResult() + self.multiTagResult.createFromPacket(packet) + targetCount = packet.decode8() + for _ in range(targetCount): + target = PhotonTrackedTarget() + target.createFromPacket(packet) + self.targets.append(target) + + return packet + + def setTimestampSeconds(self, timestampSec: float) -> None: + self.timestampSec = timestampSec + + def getLatencyMillis(self) -> float: + return self.latencyMillis + + def getTimestamp(self) -> float: + return self.timestampSec + + def getTargets(self) -> list[PhotonTrackedTarget]: + return self.targets diff --git a/photon-lib/py/photonlibpy/photonTrackedTarget.py b/photon-lib/py/photonlibpy/photonTrackedTarget.py new file mode 100644 index 0000000000..56c225d997 --- /dev/null +++ b/photon-lib/py/photonlibpy/photonTrackedTarget.py @@ -0,0 +1,82 @@ +from dataclasses import dataclass, field +from wpimath.geometry import Transform3d +from photonlibpy.packet import Packet + + +@dataclass +class TargetCorner: + x: float + y: float + + +@dataclass +class PhotonTrackedTarget: + _MAX_CORNERS = 8 + _NUM_BYTES_IN_FLOAT = 8 + _PACK_SIZE_BYTES = _NUM_BYTES_IN_FLOAT * (5 + 7 + 2 * 4 + 1 + 7 + 2 * _MAX_CORNERS) + + yaw: float = 0.0 + pitch: float = 0.0 + area: float = 0.0 + skew: float = 0.0 + fiducialId: int = -1 + bestCameraToTarget: Transform3d = field(default_factory=Transform3d) + altCameraToTarget: Transform3d = field(default_factory=Transform3d) + minAreaRectCorners: list[TargetCorner] | None = None + detectedCorners: list[TargetCorner] | None = None + poseAmbiguity: float = 0.0 + + def getYaw(self) -> float: + return self.yaw + + def getPitch(self) -> float: + return self.pitch + + def getArea(self) -> float: + return self.area + + def getSkew(self) -> float: + return self.skew + + def getFiducialId(self) -> int: + return self.fiducialId + + def getPoseAmbiguity(self) -> float: + return self.poseAmbiguity + + def getMinAreaRectCorners(self) -> list[TargetCorner] | None: + return self.minAreaRectCorners + + def getDetectedCorners(self) -> list[TargetCorner] | None: + return self.detectedCorners + + def getBestCameraToTarget(self) -> Transform3d: + return self.bestCameraToTarget + + def getAlternateCameraToTarget(self) -> Transform3d: + return self.altCameraToTarget + + def _decodeTargetList(self, packet: Packet, numTargets: int) -> list[TargetCorner]: + retList = [] + for _ in range(numTargets): + cx = packet.decodeDouble() + cy = packet.decodeDouble() + retList.append(TargetCorner(cx, cy)) + return retList + + def createFromPacket(self, packet: Packet) -> Packet: + self.yaw = packet.decodeDouble() + self.pitch = packet.decodeDouble() + self.area = packet.decodeDouble() + self.skew = packet.decodeDouble() + self.fiducialId = packet.decode32() + + self.bestCameraToTarget = packet.decodeTransform() + self.altCameraToTarget = packet.decodeTransform() + + self.poseAmbiguity = packet.decodeDouble() + + self.minAreaRectCorners = self._decodeTargetList(packet, 4) # always four + numCorners = packet.decode8() + self.detectedCorners = self._decodeTargetList(packet, numCorners) + return packet diff --git a/photon-lib/py/setup.py b/photon-lib/py/setup.py new file mode 100644 index 0000000000..b1c3a06133 --- /dev/null +++ b/photon-lib/py/setup.py @@ -0,0 +1,54 @@ +from setuptools import setup, find_packages +import subprocess, re + +gitDescribeResult = ( + subprocess.check_output(["git", "describe", "--tags", "--match=v*", "--always"]) + .decode("utf-8") + .strip() +) + +m = re.search( + r"(v[0-9]{4}\.[0-9]{1}\.[0-9]{1})-?((?:beta)?(?:alpha)?)-?([0-9\.]*)", + gitDescribeResult, +) + +# Extract the first portion of the git describe result +# which should be PEP440 compliant +if m: + versionString = m.group(0) + prefix = m.group(1) + maturity = m.group(2) + suffix = m.group(3).replace(".", "") + versionString = f"{prefix}.{maturity}.{suffix}" + + +else: + print("Warning, no valid version found") + versionString = gitDescribeResult + +print(f"Building version {versionString}") + +# Put the version info into a python file for runtime access +with open("photonlibpy/version.py", "w", encoding="utf-8") as fp: + fp.write(f'PHOTONLIB_VERSION="{versionString}"\n') + fp.write(f'PHOTONVISION_VERSION="{gitDescribeResult}"\n') + + +descriptionStr = f""" +Pure-python implementation of PhotonLib for interfacing with PhotonVision on coprocessors. +Implemented with PhotonVision version {gitDescribeResult} . +""" + +setup( + name="photonlibpy", + packages=find_packages(), + version=versionString, + install_requires=[ + "wpilib<2025,>=2024.0.0b2", + "robotpy-wpimath<2025,>=2024.0.0b2", + "pyntcore<2025,>=2024.0.0b2", + ], + description=descriptionStr, + url="https://photonvision.org", + author="Photonvision Development Team", +) diff --git a/photon-lib/py/test/data.py b/photon-lib/py/test/data.py new file mode 100644 index 0000000000..8c317dfe2f --- /dev/null +++ b/photon-lib/py/test/data.py @@ -0,0 +1,239 @@ +# fmt: off +rawBytes1 = [ + 64, 166, 117, 41, 225, 243, 165, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 0 +] +rawBytes2 = [ + 64, 114, 72, 58, 227, 96, 141, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 0 +] +rawBytes3 = [ + 64, 55, 65, 189, 215, 102, 131, 195, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 0 +] +rawBytes4 = [ + 64, 115, 23, 245, 248, 9, 145, 121, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 1, 64, 41, 32, 212, 70, 53, 253, 38, 64, 19, 140, + 198, 187, 206, 56, 251, 64, 38, 63, 170, 170, 170, 170, 170, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 17, 63, 226, 15, 19, 123, 229, 152, 236, 191, 191, 225, + 27, 96, 1, 219, 30, 63, 168, 175, 103, 65, 172, 24, 8, 63, 115, 51, 123, + 216, 202, 14, 128, 191, 155, 163, 119, 215, 217, 209, 224, 63, 212, 76, 79, + 227, 166, 197, 80, 63, 238, 85, 211, 252, 64, 132, 136, 63, 226, 13, 126, + 170, 219, 202, 209, 191, 190, 134, 80, 94, 25, 179, 17, 63, 168, 92, 53, + 102, 36, 28, 64, 63, 204, 203, 52, 12, 186, 226, 51, 63, 148, 67, 104, 89, + 131, 114, 208, 63, 211, 104, 18, 25, 149, 138, 78, 63, 237, 159, 242, 53, + 211, 204, 51, 63, 217, 254, 169, 82, 190, 36, 22, 64, 123, 96, 0, 6, 100, + 53, 178, 64, 112, 207, 255, 241, 198, 25, 18, 64, 132, 215, 255, 254, 189, + 61, 109, 64, 86, 191, 255, 217, 164, 214, 161, 64, 138, 55, 94, 60, 205, + 229, 39, 64, 115, 130, 222, 78, 57, 230, 238, 64, 131, 15, 94, 65, 66, 194, + 147, 64, 126, 162, 222, 73, 150, 202, 88, 4, 64, 130, 248, 64, 192, 0, 0, 0, + 64, 126, 15, 133, 64, 0, 0, 0, 64, 137, 206, 237, 128, 0, 0, 0, 64, 116, 48, + 240, 32, 0, 0, 0, 64, 132, 218, 43, 96, 0, 0, 0, 64, 86, 210, 155, 128, 0, + 0, 0, 64, 123, 102, 127, 192, 0, 0, 0, 64, 112, 211, 233, 96, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 +] +rawBytes5 = [ + 64, 102, 149, 235, 181, 90, 192, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 1, 64, 21, 210, 112, 148, 86, 4, 131, 64, 3, 87, + 196, 18, 174, 105, 145, 64, 47, 80, 237, 9, 123, 66, 95, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 17, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, + 0, 0, 0, 0, 64, 116, 192, 0, 0, 0, 0, 0, 64, 127, 128, 0, 0, 0, 0, 0, 64, + 116, 192, 0, 0, 0, 0, 0, 64, 100, 96, 0, 0, 0, 0, 0, 64, 133, 72, 0, 0, 0, + 0, 0, 64, 100, 96, 0, 0, 0, 0, 0, 64, 133, 72, 0, 0, 0, 0, 0, 64, 127, 128, + 0, 0, 0, 0, 0, 4, 64, 133, 78, 45, 224, 0, 0, 0, 64, 127, 129, 184, 160, 0, + 0, 0, 64, 133, 78, 1, 192, 0, 0, 0, 64, 100, 100, 194, 224, 0, 0, 0, 64, + 118, 181, 224, 64, 0, 0, 0, 64, 102, 98, 136, 0, 0, 0, 0, 64, 116, 207, 155, + 64, 0, 0, 0, 64, 126, 121, 100, 96, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0 +] +rawBytes6 = [ + 64, 78, 129, 235, 32, 116, 234, 142, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, 8, 64, 43, 213, 73, 8, 241, 221, 240, 192, 2, 146, + 71, 190, 201, 205, 25, 64, 37, 96, 141, 183, 156, 102, 0, 192, 9, 112, 76, + 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, + 240, 0, 0, 0, 0, 0, 0, 64, 101, 10, 36, 92, 234, 132, 108, 64, 89, 45, 24, + 60, 197, 216, 218, 64, 114, 7, 35, 134, 17, 204, 20, 64, 87, 129, 247, 219, + 201, 12, 226, 64, 114, 67, 177, 81, 138, 189, 202, 64, 100, 68, 236, 33, + 157, 19, 147, 64, 101, 131, 63, 243, 220, 103, 216, 64, 101, 26, 124, 82, + 27, 121, 143, 0, 192, 32, 131, 181, 181, 155, 145, 13, 192, 37, 92, 235, 61, + 221, 83, 253, 63, 173, 176, 233, 61, 133, 212, 255, 192, 73, 171, 139, 128, + 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, + 240, 0, 0, 0, 0, 0, 0, 64, 92, 42, 37, 190, 203, 146, 101, 64, 102, 61, 168, + 153, 137, 73, 186, 64, 94, 48, 100, 45, 88, 178, 51, 64, 100, 249, 193, 146, + 50, 77, 24, 64, 94, 220, 25, 65, 52, 109, 155, 64, 101, 62, 112, 102, 118, + 182, 70, 64, 92, 213, 218, 210, 167, 77, 205, 64, 102, 130, 87, 109, 205, + 178, 232, 0, 192, 19, 156, 59, 67, 88, 173, 35, 192, 37, 85, 193, 76, 240, + 22, 41, 63, 152, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, + 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, + 96, 128, 0, 0, 0, 0, 0, 64, 101, 128, 0, 0, 0, 0, 0, 64, 97, 64, 0, 0, 0, 0, + 0, 64, 101, 128, 0, 0, 0, 0, 0, 64, 97, 64, 0, 0, 0, 0, 0, 64, 101, 224, 0, + 0, 0, 0, 0, 64, 96, 128, 0, 0, 0, 0, 0, 64, 101, 224, 0, 0, 0, 0, 0, 0, 64, + 48, 164, 171, 25, 83, 4, 154, 192, 34, 178, 85, 3, 174, 51, 62, 63, 147, + 255, 255, 245, 245, 4, 85, 192, 28, 128, 4, 0, 0, 0, 0, 255, 255, 255, 255, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, + 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 110, + 24, 157, 164, 237, 183, 149, 64, 101, 4, 236, 103, 73, 0, 224, 64, 111, 24, + 157, 165, 1, 191, 69, 64, 100, 228, 236, 103, 153, 126, 38, 64, 111, 32, 0, + 27, 18, 72, 107, 64, 101, 32, 0, 24, 182, 255, 32, 64, 110, 32, 0, 26, 254, + 64, 187, 64, 101, 64, 0, 24, 102, 129, 218, 0, 63, 247, 149, 178, 38, 100, + 246, 86, 192, 9, 184, 134, 230, 194, 222, 110, 63, 150, 102, 102, 85, 138, + 188, 43, 64, 81, 228, 41, 192, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 100, 147, 51, 63, + 205, 146, 189, 64, 96, 153, 153, 160, 125, 112, 225, 64, 100, 224, 0, 12, + 123, 135, 101, 64, 96, 128, 0, 6, 164, 45, 139, 64, 101, 35, 51, 64, 50, + 109, 67, 64, 97, 73, 153, 159, 130, 143, 31, 64, 100, 214, 102, 115, 132, + 120, 155, 64, 97, 99, 51, 57, 91, 210, 117, 0, 192, 33, 131, 192, 140, 90, + 220, 117, 192, 54, 53, 227, 123, 209, 81, 175, 63, 136, 0, 0, 0, 0, 0, 0, + 64, 86, 128, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 92, 128, 0, 0, 0, 0, 0, 64, + 109, 128, 0, 0, 0, 0, 0, 64, 93, 64, 0, 0, 0, 0, 0, 64, 109, 128, 0, 0, 0, + 0, 0, 64, 93, 64, 0, 0, 0, 0, 0, 64, 109, 224, 0, 0, 0, 0, 0, 64, 92, 128, + 0, 0, 0, 0, 0, 64, 109, 224, 0, 0, 0, 0, 0, 0, 192, 25, 145, 202, 146, 13, + 244, 248, 192, 36, 180, 8, 18, 34, 149, 42, 63, 128, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 95, 128, 0, 0, 0, 0, 0, 64, 101, 96, 0, + 0, 0, 0, 0, 64, 96, 32, 0, 0, 0, 0, 0, 64, 101, 96, 0, 0, 0, 0, 0, 64, 96, + 32, 0, 0, 0, 0, 0, 64, 101, 160, 0, 0, 0, 0, 0, 64, 95, 128, 0, 0, 0, 0, 0, + 64, 101, 160, 0, 0, 0, 0, 0, 0, 192, 35, 144, 241, 16, 205, 7, 236, 192, 0, + 254, 21, 96, 184, 112, 69, 63, 135, 255, 255, 219, 151, 33, 85, 192, 70, + 128, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 63, 240, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 191, 240, 0, 0, 0, 0, 0, 0, 64, 91, 0, 0, 64, 109, 58, 156, 64, 96, + 96, 0, 31, 237, 203, 144, 64, 91, 192, 0, 63, 219, 151, 34, 64, 96, 0, 0, + 32, 54, 157, 78, 64, 92, 32, 0, 63, 146, 197, 100, 64, 96, 48, 0, 32, 18, + 52, 112, 64, 91, 96, 0, 64, 36, 104, 222, 64, 96, 144, 0, 31, 201, 98, 178, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 +] diff --git a/photon-lib/py/test/photonlibpy_test.py b/photon-lib/py/test/photonlibpy_test.py new file mode 100644 index 0000000000..92b1636351 --- /dev/null +++ b/photon-lib/py/test/photonlibpy_test.py @@ -0,0 +1,46 @@ +from photonlibpy.packet import Packet +from photonlibpy.photonPipelineResult import PhotonPipelineResult +from data import rawBytes1 +from data import rawBytes2 +from data import rawBytes3 +from data import rawBytes4 +from data import rawBytes5 +from data import rawBytes6 + + +def setupCommon(bytesIn): + res = PhotonPipelineResult() + packet = Packet(bytesIn) + res.populateFromPacket(packet) + assert packet.outOfBytes is False + return res + + +def test_byteParse1(): + res = setupCommon(rawBytes1) + assert len(res.getTargets()) == 0 + + +def test_byteParse2(): + res = setupCommon(rawBytes2) + assert len(res.getTargets()) == 0 + + +def test_byteParse3(): + res = setupCommon(rawBytes3) + assert len(res.getTargets()) == 0 + + +def test_byteParse4(): + res = setupCommon(rawBytes4) + assert len(res.getTargets()) == 1 + + +def test_byteParse5(): + res = setupCommon(rawBytes5) + assert len(res.getTargets()) == 1 + + +def test_byteParse6(): + res = setupCommon(rawBytes6) + assert len(res.getTargets()) > 6 From cba4db0bceb144cd4f178131aa09a023bf17dc1a Mon Sep 17 00:00:00 2001 From: Drew Williams Date: Sat, 16 Dec 2023 13:41:27 -0500 Subject: [PATCH 2/3] Update C++ Simulation to match Java (#1026) --- .../main/native/cpp/photon/PhotonCamera.cpp | 17 +- .../main/native/include/photon/PhotonCamera.h | 8 +- .../photon/simulation/PhotonCameraSim.h | 449 +++++++++++++++++ .../photon/simulation/SimCameraProperties.h | 475 ++++++++++++++++++ .../include/photon/simulation/VideoSimUtil.h | 432 ++++++++++++++++ .../photon/simulation/VisionSystemSim.h | 282 +++++++++++ .../photon/simulation/VisionTargetSim.h | 73 +++ .../test/native/cpp/VisionSystemSimTest.cpp | 461 +++++++++++++++++ .../photon/estimation/CameraTargetRelation.h | 61 +++ .../include/photon/estimation/OpenCVHelp.h | 291 +++++++++++ .../photon/estimation/RotTrlTransform3d.h | 102 ++++ .../include/photon/estimation/TargetModel.h | 115 +++++ .../photon/estimation/VisionEstimation.h | 121 +++++ .../include/photon/networktables/NTTopicSet.h | 100 ++++ .../include/photon/targeting/PNPResult.h | 12 +- .../swervedriveposeestsim/.gitignore | 1 + .../.wpilib/wpilib_preferences.json | 6 + .../swervedriveposeestsim/WPILib-License.md | 24 + .../swervedriveposeestsim/build.gradle | 121 +++++ .../swervedriveposeestsim/gradlew | 241 +++++++++ .../swervedriveposeestsim/gradlew.bat | 91 ++++ .../swervedriveposeestsim/networktables.json | 1 + .../swervedriveposeestsim/settings.gradle | 30 ++ .../swervedriveposeestsim/simgui-ds.json | 98 ++++ .../swervedriveposeestsim/simgui-window.json | 64 +++ .../swervedriveposeestsim/simgui.json | 57 +++ .../src/main/cpp/Robot.cpp | 123 +++++ .../src/main/cpp/subsystems/SwerveDrive.cpp | 211 ++++++++ .../main/cpp/subsystems/SwerveDriveSim.cpp | 285 +++++++++++ .../src/main/cpp/subsystems/SwerveModule.cpp | 146 ++++++ .../src/main/deploy/example.txt | 4 + .../src/main/include/Constants.h | 118 +++++ .../src/main/include/Robot.h | 62 +++ .../src/main/include/Vision.h | 152 ++++++ .../src/main/include/subsystems/SwerveDrive.h | 85 ++++ .../main/include/subsystems/SwerveDriveSim.h | 102 ++++ .../main/include/subsystems/SwerveModule.h | 81 +++ .../src/test/cpp/main.cpp | 34 ++ .../swervedriveposeestsim/swerve_module.png | Bin 0 -> 2954 bytes .../swervedriveposeestsim/tag-blue.png | Bin 0 -> 4711 bytes .../swervedriveposeestsim/tag-green.png | Bin 0 -> 4698 bytes shared/javacpp/setupBuild.gradle | 2 + 42 files changed, 5123 insertions(+), 15 deletions(-) create mode 100644 photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h create mode 100644 photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h create mode 100644 photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h create mode 100644 photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h create mode 100644 photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h create mode 100644 photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp create mode 100644 photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h create mode 100644 photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h create mode 100644 photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h create mode 100644 photon-targeting/src/main/native/include/photon/estimation/TargetModel.h create mode 100644 photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h create mode 100644 photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/.gitignore create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/build.gradle create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/gradlew create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/networktables.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/settings.gradle create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/simgui.json create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png create mode 100644 photonlib-cpp-examples/swervedriveposeestsim/tag-green.png diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 95eecb444c..ff26f74fb8 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include "PhotonVersion.h" @@ -128,8 +129,13 @@ LEDMode PhotonCamera::GetLEDMode() const { std::optional PhotonCamera::GetCameraMatrix() { auto camCoeffs = cameraIntrinsicsSubscriber.Get(); if (camCoeffs.size() == 9) { - // clone should deal with ownership concerns? not sure - return cv::Mat(3, 3, CV_64FC1, camCoeffs.data()).clone(); + cv::Mat retVal(3, 3, CV_64FC1); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + retVal.at(i, j) = camCoeffs[(j * 3) + i]; + } + } + return retVal; } return std::nullopt; } @@ -145,8 +151,11 @@ const std::string_view PhotonCamera::GetCameraName() const { std::optional PhotonCamera::GetDistCoeffs() { auto distCoeffs = cameraDistortionSubscriber.Get(); if (distCoeffs.size() == 5) { - // clone should deal with ownership concerns? not sure - return cv::Mat(5, 1, CV_64FC1, distCoeffs.data()).clone(); + cv::Mat retVal(5, 1, CV_64FC1); + for (int i = 0; i < 5; i++) { + retVal.at(i, 0) = distCoeffs[i]; + } + return retVal; } return std::nullopt; } diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 2a1418f1b5..582458e014 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -39,11 +39,7 @@ #include #include -#include "photon/dataflow/structures/Packet.h" -#include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" -#include "photon/targeting/PhotonPipelineResult.h" -#include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting//PhotonPipelineResult.h" namespace cv { class Mat; @@ -172,6 +168,8 @@ class PhotonCamera { PhotonCamera::VERSION_CHECK_ENABLED = enabled; } + std::shared_ptr GetCameraTable() const { return rootTable; } + // For use in tests bool test = false; PhotonPipelineResult testResult; diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h new file mode 100644 index 0000000000..367b30bb8f --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -0,0 +1,449 @@ +/* + * MIT License + * + * Copyright (c) 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace photon { +class PhotonCameraSim { + public: + explicit PhotonCameraSim(PhotonCamera* camera) + : PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG()) {} + PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props) + : prop(props), cam(camera) { + SetMinTargetAreaPixels(kDefaultMinAreaPx); + videoSimRaw = frc::CameraServer::PutVideo( + std::string{camera->GetCameraName()} + "-raw", prop.GetResWidth(), + prop.GetResHeight()); + videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray); + videoSimProcessed = frc::CameraServer::PutVideo( + std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(), + prop.GetResHeight()); + ts.subTable = cam->GetCameraTable(); + ts.UpdateEntries(); + } + PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props, + double minTargetAreaPercent, units::meter_t maxSightRange) + : PhotonCameraSim(camera, props) { + this->minTargetAreaPercent = minTargetAreaPercent; + this->maxSightRange = maxSightRange; + } + PhotonCamera* GetCamera() { return cam; } + double GetMinTargetAreaPercent() { return minTargetAreaPercent; } + double GetMinTargetAreaPixels() { + return minTargetAreaPercent / 100.0 * prop.GetResArea(); + } + units::meter_t GetMaxSightRange() { return maxSightRange; } + const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; } + const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; } + bool CanSeeTargetPose(const frc::Pose3d& camPose, + const VisionTargetSim& target) { + CameraTargetRelation rel{camPose, target.GetPose()}; + return ((units::math::abs(rel.camToTargYaw.Degrees()) < + prop.GetHorizFOV().Degrees() / 2.0) && + (units::math::abs(rel.camToTargPitch.Degrees()) < + prop.GetVertFOV().Degrees() / 2.0) && + (!target.GetModel().GetIsPlanar() || + units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) && + (rel.camToTarg.Translation().Norm() <= maxSightRange)); + } + bool CanSeeCorner(const std::vector& points) { + for (const auto& pt : points) { + if (std::clamp(pt.x, 0, prop.GetResWidth()) != pt.x || + std::clamp(pt.y, 0, prop.GetResHeight()) != pt.y) { + return false; + } + } + return true; + } + std::optional ConsumeNextEntryTime() { + uint64_t now = wpi::Now(); + uint64_t timestamp{}; + int iter = 0; + while (now >= nextNTEntryTime) { + timestamp = nextNTEntryTime; + uint64_t frameTime = prop.EstSecUntilNextFrame() + .convert() + .to(); + nextNTEntryTime += frameTime; + + if (iter++ > 50) { + timestamp = now; + nextNTEntryTime = now + frameTime; + break; + } + } + + if (timestamp != 0) { + return timestamp; + } else { + return std::nullopt; + } + } + void SetMinTargetAreaPercent(double areaPercent) { + minTargetAreaPercent = areaPercent; + } + void SetMinTargetAreaPixels(double areaPx) { + minTargetAreaPercent = areaPx / prop.GetResArea() * 100; + } + void SetMaxSightRange(units::meter_t range) { maxSightRange = range; } + void EnableRawStream(bool enabled) { videoSimRawEnabled = enabled; } + void EnableDrawWireframe(bool enabled) { videoSimWireframeEnabled = enabled; } + void SetWireframeResolution(double resolution) { + videoSimWireframeResolution = resolution; + } + void EnabledProcessedStream(double enabled) { videoSimProcEnabled = enabled; } + PhotonPipelineResult Process(units::second_t latency, + const frc::Pose3d& cameraPose, + std::vector targets) { + std::sort( + targets.begin(), targets.end(), + [cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) { + units::meter_t dist1 = + t1.GetPose().Translation().Distance(cameraPose.Translation()); + units::meter_t dist2 = + t2.GetPose().Translation().Distance(cameraPose.Translation()); + return dist1 > dist2; + }); + + std::vector>> + visibleTgts{}; + std::vector detectableTgts{}; + RotTrlTransform3d camRt = RotTrlTransform3d::MakeRelativeTo(cameraPose); + + VideoSimUtil::UpdateVideoProp(videoSimRaw, prop); + VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop); + cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()}; + cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1); + blankFrame.assignTo(videoSimFrameRaw); + + for (const auto& tgt : targets) { + if (!CanSeeTargetPose(cameraPose, tgt)) { + continue; + } + + std::vector fieldCorners = tgt.GetFieldVertices(); + if (tgt.GetModel().GetIsSpherical()) { + TargetModel model = tgt.GetModel(); + fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose( + tgt.GetPose().Translation(), cameraPose.Translation())); + } + + std::vector imagePoints = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, fieldCorners); + if (tgt.GetModel().GetIsSpherical()) { + cv::Point2d center = OpenCVHelp::AvgPoint(imagePoints); + int l = 0; + int t = 0; + int b = 0; + int r = 0; + for (int i = 0; i < 4; i++) { + if (imagePoints[i].x < imagePoints[l].x) { + l = i; + } + } + cv::Point2d lc = imagePoints[l]; + std::array angles{}; + t = (l + 1) % 4; + b = (l + 1) % 4; + for (int i = 0; i < 4; i++) { + if (i == l) { + continue; + } + cv::Point2d ic = imagePoints[i]; + angles[i] = std::atan2(lc.y - ic.y, ic.x - lc.x); + if (angles[i] >= angles[t]) { + t = i; + } + if (angles[i] <= angles[b]) { + b = i; + } + } + for (int i = 0; i < 4; i++) { + if (i != t && i != l && i != b) { + r = i; + } + } + cv::RotatedRect rect{ + cv::Point2d{center.x, center.y}, + cv::Size2d{imagePoints[r].x - lc.x, + imagePoints[b].y - imagePoints[t].y}, + units::radian_t{-angles[r]}.convert().to()}; + std::vector points{}; + rect.points(points); + + // Can't find an easier way to convert from Point2f to Point2d + imagePoints.clear(); + std::transform(points.begin(), points.end(), + std::back_inserter(imagePoints), + [](const cv::Point2f& p) { return (cv::Point2d)p; }); + } + + visibleTgts.emplace_back(std::make_pair(tgt, imagePoints)); + std::vector noisyTargetCorners = + prop.EstPixelNoise(imagePoints); + cv::RotatedRect minAreaRect = + OpenCVHelp::GetMinAreaRect(noisyTargetCorners); + std::vector minAreaRectPts; + minAreaRectPts.reserve(4); + minAreaRect.points(minAreaRectPts); + cv::Point2d centerPt = minAreaRect.center; + frc::Rotation3d centerRot = prop.GetPixelRot(centerPt); + double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners); + + if (!(CanSeeCorner(noisyTargetCorners) && + areaPercent >= minTargetAreaPercent)) { + continue; + } + + PNPResult pnpSim{}; + if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) { + pnpSim = OpenCVHelp::SolvePNP_Square( + prop.GetIntrinsics(), prop.GetDistCoeffs(), + tgt.GetModel().GetVertices(), noisyTargetCorners); + } + + std::vector> tempCorners = + OpenCVHelp::PointsToCorners(minAreaRectPts); + wpi::SmallVector, 4> smallVec; + + for (const auto& corner : tempCorners) { + smallVec.emplace_back( + std::make_pair(static_cast(corner.first), + static_cast(corner.second))); + } + + std::vector> cornersFloat = + OpenCVHelp::PointsToCorners(noisyTargetCorners); + + std::vector> cornersDouble{cornersFloat.begin(), + cornersFloat.end()}; + detectableTgts.emplace_back(PhotonTrackedTarget{ + centerRot.Z().convert().to(), + -centerRot.Y().convert().to(), areaPercent, + centerRot.X().convert().to(), tgt.fiducialId, + pnpSim.best, pnpSim.alt, pnpSim.ambiguity, smallVec, cornersDouble}); + } + + if (videoSimRawEnabled) { + if (videoSimWireframeEnabled) { + VideoSimUtil::DrawFieldWireFrame( + camRt, prop, videoSimWireframeResolution, 1.5, cv::Scalar{80}, 6, 1, + cv::Scalar{30}, videoSimFrameRaw); + } + + for (const auto& pair : visibleTgts) { + VisionTargetSim tgt = pair.first; + std::vector corners = pair.second; + + if (tgt.fiducialId > 0) { + VideoSimUtil::Warp165h5TagImage(tgt.fiducialId, corners, true, + videoSimFrameRaw); + } else if (!tgt.GetModel().GetIsSpherical()) { + std::vector contour = corners; + if (!tgt.GetModel().GetIsPlanar()) { + contour = OpenCVHelp::GetConvexHull(contour); + } + VideoSimUtil::DrawPoly(contour, -1, cv::Scalar{255}, true, + videoSimFrameRaw); + } else { + VideoSimUtil::DrawInscribedEllipse(corners, cv::Scalar{255}, + videoSimFrameRaw); + } + } + videoSimRaw.PutFrame(videoSimFrameRaw); + } else { + videoSimRaw.SetConnectionStrategy( + cs::VideoSource::ConnectionStrategy::kConnectionForceClose); + } + + if (videoSimProcEnabled) { + cv::cvtColor(videoSimFrameRaw, videoSimFrameProcessed, + cv::COLOR_GRAY2BGR); + cv::drawMarker( + videoSimFrameProcessed, + cv::Point2d{prop.GetResWidth() / 2.0, prop.GetResHeight() / 2.0}, + cv::Scalar{0, 255, 0}, cv::MARKER_CROSS, + static_cast( + VideoSimUtil::GetScaledThickness(15, videoSimFrameProcessed)), + static_cast( + VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)), + cv::LINE_AA); + for (const auto& tgt : detectableTgts) { + auto detectedCornersDouble = tgt.GetDetectedCorners(); + std::vector> detectedCornerFloat{ + detectedCornersDouble.begin(), detectedCornersDouble.end()}; + if (tgt.GetFiducialId() >= 0) { + VideoSimUtil::DrawTagDetection( + tgt.GetFiducialId(), + OpenCVHelp::CornersToPoints(detectedCornerFloat), + videoSimFrameProcessed); + } else { + cv::rectangle(videoSimFrameProcessed, + OpenCVHelp::GetBoundingRect( + OpenCVHelp::CornersToPoints(detectedCornerFloat)), + cv::Scalar{0, 0, 255}, + static_cast(VideoSimUtil::GetScaledThickness( + 1, videoSimFrameProcessed)), + cv::LINE_AA); + + wpi::SmallVector, 4> smallVec = + tgt.GetMinAreaRectCorners(); + + std::vector> cornersCopy{}; + cornersCopy.reserve(4); + + for (const auto& corner : smallVec) { + cornersCopy.emplace_back( + std::make_pair(static_cast(corner.first), + static_cast(corner.second))); + } + + VideoSimUtil::DrawPoly( + OpenCVHelp::CornersToPoints(cornersCopy), + static_cast( + VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)), + cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed); + } + } + videoSimProcessed.PutFrame(videoSimFrameProcessed); + } else { + videoSimProcessed.SetConnectionStrategy( + cs::VideoSource::ConnectionStrategy::kConnectionForceClose); + } + + MultiTargetPNPResult multiTagResults{}; + + std::vector visibleLayoutTags = + VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout); + if (visibleLayoutTags.size() > 1) { + wpi::SmallVector usedIds{}; + std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(), + usedIds.begin(), + [](const frc::AprilTag& tag) { return tag.ID; }); + std::sort(usedIds.begin(), usedIds.end()); + PNPResult pnpResult = VisionEstimation::EstimateCamPosePNP( + prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout, + kAprilTag16h5); + multiTagResults = MultiTargetPNPResult{pnpResult, usedIds}; + } + + return PhotonPipelineResult{latency, detectableTgts, multiTagResults}; + } + void SubmitProcessedFrame(const PhotonPipelineResult& result) { + SubmitProcessedFrame(result, wpi::Now()); + } + void SubmitProcessedFrame(const PhotonPipelineResult& result, + uint64_t recieveTimestamp) { + ts.latencyMillisEntry.Set( + result.GetLatency().convert().to(), + recieveTimestamp); + + Packet newPacket{}; + newPacket << result; + ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp); + + bool hasTargets = result.HasTargets(); + ts.hasTargetEntry.Set(hasTargets, recieveTimestamp); + if (!hasTargets) { + ts.targetPitchEntry.Set(0.0, recieveTimestamp); + ts.targetYawEntry.Set(0.0, recieveTimestamp); + ts.targetAreaEntry.Set(0.0, recieveTimestamp); + std::array poseData{0.0, 0.0, 0.0}; + ts.targetPoseEntry.Set(poseData, recieveTimestamp); + ts.targetSkewEntry.Set(0.0, recieveTimestamp); + } else { + PhotonTrackedTarget bestTarget = result.GetBestTarget(); + + ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp); + ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp); + ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp); + ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp); + + frc::Transform3d transform = bestTarget.GetBestCameraToTarget(); + std::array poseData{ + transform.X().to(), transform.Y().to(), + transform.Rotation().ToRotation2d().Degrees().to()}; + ts.targetPoseEntry.Set(poseData, recieveTimestamp); + } + + auto intrinsics = prop.GetIntrinsics(); + std::vector intrinsicsView{intrinsics.data(), + intrinsics.data() + intrinsics.size()}; + ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp); + + auto distortion = prop.GetDistCoeffs(); + std::vector distortionView{distortion.data(), + distortion.data() + distortion.size()}; + ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp); + + ts.heartbeatPublisher.Set(heartbeatCounter++, recieveTimestamp); + } + SimCameraProperties prop; + + private: + PhotonCamera* cam; + + NTTopicSet ts{}; + uint64_t heartbeatCounter{0}; + + uint64_t nextNTEntryTime{wpi::Now()}; + + units::meter_t maxSightRange{std::numeric_limits::max()}; + static constexpr double kDefaultMinAreaPx{100}; + double minTargetAreaPercent; + + frc::AprilTagFieldLayout tagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)}; + + cs::CvSource videoSimRaw; + cv::Mat videoSimFrameRaw{}; + bool videoSimRawEnabled{true}; + bool videoSimWireframeEnabled{false}; + double videoSimWireframeResolution{0.1}; + cs::CvSource videoSimProcessed; + cv::Mat videoSimFrameProcessed{}; + bool videoSimProcEnabled{true}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h new file mode 100644 index 0000000000..f3a5d73677 --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h @@ -0,0 +1,475 @@ +/* + * MIT License + * + * Copyright (c) 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 + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace photon { +class SimCameraProperties { + public: + SimCameraProperties() { SetCalibration(960, 720, frc::Rotation2d{90_deg}); } + SimCameraProperties(std::string path, int width, int height) {} + void SetCalibration(int width, int height, frc::Rotation2d fovDiag) { + if (fovDiag.Degrees() < 1_deg || fovDiag.Degrees() > 179_deg) { + fovDiag = frc::Rotation2d{ + std::clamp(fovDiag.Degrees(), 1_deg, 179_deg)}; + FRC_ReportError( + frc::err::Error, + "Requested invalid FOV! Clamping between (1, 179) degrees..."); + } + double resDiag = std::sqrt(width * width + height * height); + double diagRatio = units::math::tan(fovDiag.Radians() / 2.0); + frc::Rotation2d fovWidth{ + units::radian_t{std::atan(diagRatio * (width / resDiag)) * 2}}; + frc::Rotation2d fovHeight{ + units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}}; + + Eigen::Matrix newDistCoeffs; + newDistCoeffs << 0, 0, 0, 0, 0; + + double cx = width / 2.0 - 0.5; + double cy = height / 2.0 - 0.5; + + double fx = cx / units::math::tan(fovWidth.Radians() / 2.0); + double fy = cy / units::math::tan(fovHeight.Radians() / 2.0); + + Eigen::Matrix newCamIntrinsics; + newCamIntrinsics << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0; + SetCalibration(width, height, newCamIntrinsics, newDistCoeffs); + } + + void SetCalibration(int width, int height, + const Eigen::Matrix& newCamIntrinsics, + const Eigen::Matrix& newDistCoeffs) { + resWidth = width; + resHeight = height; + camIntrinsics = newCamIntrinsics; + distCoeffs = newDistCoeffs; + + std::array p{ + frc::Translation3d{ + 1_m, + frc::Rotation3d{0_rad, 0_rad, + (GetPixelYaw(0) + frc::Rotation2d{units::radian_t{ + -std::numbers::pi / 2.0}}) + .Radians()}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, 0_rad, + (GetPixelYaw(width) + + frc::Rotation2d{ + units::radian_t{std::numbers::pi / 2.0}}) + .Radians()}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, + (GetPixelPitch(0) + + frc::Rotation2d{ + units::radian_t{std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, + frc::Translation3d{ + 1_m, frc::Rotation3d{0_rad, + (GetPixelPitch(height) + + frc::Rotation2d{ + units::radian_t{-std::numbers::pi / 2.0}}) + .Radians(), + 0_rad}}, + }; + viewplanes.clear(); + for (size_t i = 0; i < p.size(); i++) { + viewplanes.emplace_back(Eigen::Matrix{ + p[i].X().to(), p[i].Y().to(), p[i].Z().to()}); + } + } + + void SetCalibError(double newAvgErrorPx, double newErrorStdDevPx) { + avgErrorPx = newAvgErrorPx; + errorStdDevPx = newErrorStdDevPx; + } + + void SetFPS(units::hertz_t fps) { + frameSpeed = units::math::max(1 / fps, exposureTime); + } + + void SetExposureTime(units::second_t newExposureTime) { + exposureTime = newExposureTime; + frameSpeed = units::math::max(frameSpeed, exposureTime); + } + + void SetAvgLatency(units::second_t newAvgLatency) { + avgLatency = newAvgLatency; + } + + void SetLatencyStdDev(units::second_t newLatencyStdDev) { + latencyStdDev = newLatencyStdDev; + } + + int GetResWidth() const { return resWidth; } + + int GetResHeight() const { return resHeight; } + + int GetResArea() const { return resWidth * resHeight; } + + double GetAspectRatio() const { + return static_cast(resWidth) / static_cast(resHeight); + } + + Eigen::Matrix GetIntrinsics() const { return camIntrinsics; } + + Eigen::Matrix GetDistCoeffs() const { return distCoeffs; } + + units::hertz_t GetFPS() const { return 1 / frameSpeed; } + + units::second_t GetFrameSpeed() const { return frameSpeed; } + + units::second_t GetExposureTime() const { return exposureTime; } + + units::second_t GetAverageLatency() const { return avgLatency; } + + units::second_t GetLatencyStdDev() const { return latencyStdDev; } + + double GetContourAreaPercent(const std::vector& points) { + return cv::contourArea(photon::OpenCVHelp::GetConvexHull(points)) / + GetResArea() * 100; + } + + frc::Rotation2d GetPixelYaw(double pixelX) const { + double fx = camIntrinsics(0, 0); + double cx = camIntrinsics(0, 2); + double xOffset = cx - pixelX; + return frc::Rotation2d{fx, xOffset}; + } + + frc::Rotation2d GetPixelPitch(double pixelY) const { + double fy = camIntrinsics(1, 1); + double cy = camIntrinsics(1, 2); + double yOffset = cy - pixelY; + return frc::Rotation2d{fy, -yOffset}; + } + + frc::Rotation3d GetPixelRot(const cv::Point2d& point) const { + return frc::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(), + GetPixelYaw(point.x).Radians()}; + } + + frc::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const { + double fx = camIntrinsics(0, 0); + double cx = camIntrinsics(0, 2); + double xOffset = cx - point.x; + + double fy = camIntrinsics(1, 1); + double cy = camIntrinsics(1, 2); + double yOffset = cy - point.y; + + frc::Rotation2d yaw{fx, xOffset}; + frc::Rotation2d pitch{fy / std::cos(std::atan(xOffset / fx)), -yOffset}; + return frc::Rotation3d{0_rad, pitch.Radians(), yaw.Radians()}; + } + + frc::Rotation2d GetHorizFOV() const { + frc::Rotation2d left = GetPixelYaw(0); + frc::Rotation2d right = GetPixelYaw(resWidth); + return left - right; + } + + frc::Rotation2d GetVertFOV() const { + frc::Rotation2d above = GetPixelPitch(0); + frc::Rotation2d below = GetPixelPitch(resHeight); + return below - above; + } + + frc::Rotation2d GetDiagFOV() const { + return frc::Rotation2d{ + units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())}; + } + + std::pair, std::optional> GetVisibleLine( + const RotTrlTransform3d& camRt, const frc::Translation3d& a, + const frc::Translation3d& b) const { + frc::Translation3d relA = camRt.Apply(a); + frc::Translation3d relB = camRt.Apply(b); + + if (relA.X() <= 0_m && relB.X() <= 0_m) { + return {std::nullopt, std::nullopt}; + } + + Eigen::Matrix av{relA.X().to(), relA.Y().to(), + relA.Z().to()}; + Eigen::Matrix bv{relB.X().to(), relB.Y().to(), + relB.Z().to()}; + Eigen::Matrix abv = bv - av; + + bool aVisible = true; + bool bVisible = true; + for (size_t i = 0; i < viewplanes.size(); i++) { + Eigen::Matrix normal = viewplanes[i]; + double aVisibility = av.dot(normal); + if (aVisibility < 0) { + aVisible = false; + } + double bVisibility = bv.dot(normal); + if (bVisibility < 0) { + bVisible = false; + } + if (aVisibility <= 0 && bVisibility <= 0) { + return {std::nullopt, std::nullopt}; + } + } + + if (aVisible && bVisible) { + return {0, 1}; + } + + std::array intersections{std::nan(""), std::nan(""), + std::nan(""), std::nan("")}; + std::vector>> ipts{ + {std::nullopt, std::nullopt, std::nullopt, std::nullopt}}; + + for (size_t i = 0; i < viewplanes.size(); i++) { + Eigen::Matrix normal = viewplanes[i]; + Eigen::Matrix a_projn{}; + a_projn = (av.dot(normal) / normal.dot(normal)) * normal; + + if (std::abs(abv.dot(normal)) < 1e-5) { + continue; + } + intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn)); + + Eigen::Matrix apv{}; + apv = intersections[i] * abv; + Eigen::Matrix intersectpt{}; + intersectpt = av + apv; + ipts[i] = intersectpt; + + for (size_t j = 1; j < viewplanes.size(); j++) { + int oi = (i + j) % viewplanes.size(); + Eigen::Matrix onormal = viewplanes[oi]; + if (intersectpt.dot(onormal) < 0) { + intersections[i] = std::nan(""); + ipts[i] = std::nullopt; + break; + } + } + + if (!ipts[i]) { + continue; + } + + for (int j = i - 1; j >= 0; j--) { + std::optional> oipt = ipts[j]; + if (!oipt) { + continue; + } + Eigen::Matrix diff{}; + diff = oipt.value() - intersectpt; + if (diff.cwiseAbs().maxCoeff() < 1e-4) { + intersections[i] = std::nan(""); + ipts[i] = std::nullopt; + break; + } + } + } + + double inter1 = std::nan(""); + double inter2 = std::nan(""); + for (double inter : intersections) { + if (!std::isnan(inter)) { + if (std::isnan(inter1)) { + inter1 = inter; + } else { + inter2 = inter; + } + } + } + + if (!std::isnan(inter2)) { + double max = std::max(inter1, inter2); + double min = std::min(inter1, inter2); + if (aVisible) { + min = 0; + } + if (bVisible) { + max = 1; + } + return {min, max}; + } else if (!std::isnan(inter1)) { + if (aVisible) { + return {0, inter1}; + } + if (bVisible) { + return {inter1, 1}; + } + return {inter1, std::nullopt}; + } else { + return {std::nullopt, std::nullopt}; + } + } + + std::vector EstPixelNoise( + const std::vector& points) { + if (avgErrorPx == 0 && errorStdDevPx == 0) { + return points; + } + + std::vector noisyPts; + noisyPts.reserve(points.size()); + for (size_t i = 0; i < points.size(); i++) { + cv::Point2f p = points[i]; + float error = avgErrorPx + gaussian(generator) * errorStdDevPx; + float errorAngle = + uniform(generator) * 2 * std::numbers::pi - std::numbers::pi; + noisyPts.emplace_back(cv::Point2f{p.x + error * std::cos(errorAngle), + p.y + error * std::sin(errorAngle)}); + } + return noisyPts; + } + + units::second_t EstLatency() { + return units::math::max(avgLatency + gaussian(generator) * latencyStdDev, + 0_s); + } + + units::second_t EstSecUntilNextFrame() { + return frameSpeed + units::math::max(0_s, EstLatency() - frameSpeed); + } + + static SimCameraProperties PERFECT_90DEG() { return SimCameraProperties{}; } + + static SimCameraProperties PI4_LIFECAM_320_240() { + SimCameraProperties prop{}; + prop.SetCalibration( + 320, 240, + (Eigen::MatrixXd(3, 3) << 328.2733242048587, 0.0, 164.8190261141906, + 0.0, 318.0609794305216, 123.8633838438093, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{ + 0.09957946553445934, -0.9166265114485799, 0.0019519890627236526, + -0.0036071725380870333, 1.5627234622420942}); + prop.SetCalibError(0.21, 0.0124); + prop.SetFPS(30_Hz); + prop.SetAvgLatency(30_ms); + prop.SetLatencyStdDev(10_ms); + return prop; + } + + static SimCameraProperties PI4_LIFECAM_640_480() { + SimCameraProperties prop{}; + prop.SetCalibration( + 640, 480, + (Eigen::MatrixXd(3, 3) << 669.1428078983059, 0.0, 322.53377249329213, + 0.0, 646.9843137061716, 241.26567383784163, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{ + 0.12788470750464645, -1.2350335805796528, 0.0024990767286192732, + -0.0026958287600230705, 2.2951386729115537}); + prop.SetCalibError(0.26, 0.046); + prop.SetFPS(15_Hz); + prop.SetAvgLatency(65_ms); + prop.SetLatencyStdDev(15_ms); + return prop; + } + + static SimCameraProperties LL2_640_480() { + SimCameraProperties prop{}; + prop.SetCalibration( + 640, 480, + (Eigen::MatrixXd(3, 3) << 511.22843367007755, 0.0, 323.62049380211096, + 0.0, 514.5452336723849, 261.8827920543568, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.1917469998873756, -0.5142936883324216, + 0.012461562046896614, 0.0014084973492408186, + 0.35160648971214437}); + prop.SetCalibError(0.25, 0.05); + prop.SetFPS(15_Hz); + prop.SetAvgLatency(35_ms); + prop.SetLatencyStdDev(8_ms); + return prop; + } + + static SimCameraProperties LL2_960_720() { + SimCameraProperties prop{}; + prop.SetCalibration( + 960, 720, + (Eigen::MatrixXd(3, 3) << 769.6873145148892, 0.0, 486.1096609458122, + 0.0, 773.8164483705323, 384.66071662358354, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.189462064814501, -0.49903003669627627, + 0.007468423590519429, 0.002496885298683693, + 0.3443122090208624}); + prop.SetCalibError(0.35, 0.10); + prop.SetFPS(10_Hz); + prop.SetAvgLatency(50_ms); + prop.SetLatencyStdDev(15_ms); + return prop; + } + + static SimCameraProperties LL2_1280_720() { + SimCameraProperties prop{}; + prop.SetCalibration( + 1280, 720, + (Eigen::MatrixXd(3, 3) << 1011.3749416937393, 0.0, 645.4955139388737, + 0.0, 1008.5391755084075, 508.32877656020196, 0.0, 0.0, 1.0) + .finished(), + Eigen::Matrix{0.13730101577061535, -0.2904345656989261, + 8.32475714507539E-4, -3.694397782014239E-4, + 0.09487962227027584}); + prop.SetCalibError(0.37, 0.06); + prop.SetFPS(7_Hz); + prop.SetAvgLatency(60_ms); + prop.SetLatencyStdDev(20_ms); + return prop; + } + + private: + std::mt19937 generator{std::random_device{}()}; + std::normal_distribution gaussian{0.0, 1.0}; + std::uniform_real_distribution uniform{0.0, 1.0}; + + int resWidth; + int resHeight; + Eigen::Matrix camIntrinsics; + Eigen::Matrix distCoeffs; + double avgErrorPx; + double errorStdDevPx; + units::second_t frameSpeed{0}; + units::second_t exposureTime{0}; + units::second_t avgLatency{0}; + units::second_t latencyStdDev{0}; + std::vector> viewplanes{}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h new file mode 100644 index 0000000000..bb2c453a88 --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h @@ -0,0 +1,432 @@ +/* + * MIT License + * + * Copyright (c) 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 + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "SimCameraProperties.h" +#include "photon/estimation/RotTrlTransform3d.h" + +namespace mathutil { +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} +} // namespace mathutil + +namespace photon { +namespace VideoSimUtil { +static constexpr int kNumTags16h5 = 30; + +static constexpr units::meter_t fieldLength{16.54175_m}; +static constexpr units::meter_t fieldWidth{8.0137_m}; + +static cv::Mat Get16h5TagImage(int id) { + wpi::RawFrame frame = frc::AprilTag::Generate16h5AprilTagImage(id); + cv::Mat markerImage{frame.height, frame.width, CV_8UC1, frame.data}; + cv::Mat markerClone = markerImage.colRange(0, frame.dataLength).clone(); + return markerClone; +} + +static std::unordered_map LoadAprilTagImages() { + std::unordered_map retVal{}; + for (int i = 0; i < kNumTags16h5; i++) { + cv::Mat tagImage = Get16h5TagImage(i); + retVal[i] = tagImage; + } + return retVal; +} + +static std::vector GetImageCorners(const cv::Size& size) { + std::vector retVal{}; + retVal.emplace_back(cv::Point2f{-0.5f, -0.5f}); + retVal.emplace_back(cv::Point2f{size.width - 0.5f, -0.5f}); + retVal.emplace_back(cv::Point2f{size.width - 0.5f, size.height - 0.5f}); + retVal.emplace_back(cv::Point2f{-0.5f, size.height - 0.5f}); + return retVal; +} + +static std::vector Get16h5MarkerPts(int scale) { + cv::Rect2f roi16h5{cv::Point2f{1, 1}, cv::Point2f{6, 6}}; + roi16h5.x *= scale; + roi16h5.y *= scale; + roi16h5.width *= scale; + roi16h5.height *= scale; + std::vector pts = GetImageCorners(roi16h5.size()); + for (size_t i = 0; i < pts.size(); i++) { + cv::Point2f pt = pts[i]; + pts[i] = cv::Point2f{roi16h5.tl().x + pt.x, roi16h5.tl().y + pt.y}; + } + return pts; +} + +static std::vector Get16h5MarkerPts() { + return Get16h5MarkerPts(1); +} + +static const std::unordered_map kTag16h5Images = + LoadAprilTagImages(); +static const std::vector kTag16h5MarkPts = Get16h5MarkerPts(); + +[[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video, + const SimCameraProperties& prop) { + video.SetResolution(prop.GetResWidth(), prop.GetResHeight()); + video.SetFPS(prop.GetFPS().to()); +} + +[[maybe_unused]] static void Warp165h5TagImage( + int tagId, const std::vector& dstPoints, bool antialiasing, + cv::Mat& destination) { + if (!kTag16h5Images.contains(tagId)) { + return; + } + cv::Mat tagImage = kTag16h5Images.at(tagId); + std::vector tagPoints{kTag16h5MarkPts}; + std::vector tagImageCorners{GetImageCorners(tagImage.size())}; + std::vector dstPointMat = dstPoints; + cv::Rect boundingRect = cv::boundingRect(dstPointMat); + cv::Mat perspecTrf = cv::getPerspectiveTransform(tagPoints, dstPointMat); + std::vector extremeCorners{}; + cv::perspectiveTransform(tagImageCorners, extremeCorners, perspecTrf); + boundingRect = cv::boundingRect(extremeCorners); + + double warpedContourArea = cv::contourArea(extremeCorners); + double warpedTagUpscale = + std::sqrt(warpedContourArea) / std::sqrt(tagImage.size().area()); + int warpStrat = cv::INTER_NEAREST; + + int supersampling = 6; + supersampling = static_cast(std::ceil(supersampling / warpedTagUpscale)); + supersampling = std::max(std::min(supersampling, 8), 1); + + cv::Mat scaledTagImage{}; + if (warpedTagUpscale > 2.0) { + warpStrat = cv::INTER_LINEAR; + int scaleFactor = static_cast(warpedTagUpscale / 3.0) + 2; + scaleFactor = std::max(std::min(scaleFactor, 40), 1); + scaleFactor *= supersampling; + cv::resize(tagImage, scaledTagImage, cv::Size{}, scaleFactor, scaleFactor, + cv::INTER_NEAREST); + tagPoints = Get16h5MarkerPts(scaleFactor); + } else { + scaledTagImage = tagImage; + } + + boundingRect.x -= 1; + boundingRect.y -= 1; + boundingRect.width += 2; + boundingRect.height += 2; + if (boundingRect.x < 0) { + boundingRect.width += boundingRect.x; + boundingRect.x = 0; + } + if (boundingRect.y < 0) { + boundingRect.height += boundingRect.y; + boundingRect.y = 0; + } + boundingRect.width = + std::min(destination.size().width - boundingRect.x, boundingRect.width); + boundingRect.height = + std::min(destination.size().height - boundingRect.y, boundingRect.height); + if (boundingRect.width <= 0 || boundingRect.height <= 0) { + return; + } + + std::vector scaledDstPts{}; + if (supersampling > 1) { + cv::multiply(dstPointMat, + cv::Scalar{static_cast(supersampling), + static_cast(supersampling)}, + scaledDstPts); + boundingRect.x *= supersampling; + boundingRect.y *= supersampling; + boundingRect.width *= supersampling; + boundingRect.height *= supersampling; + } else { + scaledDstPts = dstPointMat; + } + + cv::subtract(scaledDstPts, + cv::Scalar{static_cast(boundingRect.tl().x), + static_cast(boundingRect.tl().y)}, + scaledDstPts); + perspecTrf = cv::getPerspectiveTransform(tagPoints, scaledDstPts); + + cv::Mat tempRoi{}; + cv::warpPerspective(scaledTagImage, tempRoi, perspecTrf, boundingRect.size(), + warpStrat); + + if (supersampling > 1) { + boundingRect.x /= supersampling; + boundingRect.y /= supersampling; + boundingRect.width /= supersampling; + boundingRect.height /= supersampling; + cv::resize(tempRoi, tempRoi, boundingRect.size(), 0, 0, cv::INTER_AREA); + } + + cv::Mat tempMask{cv::Mat::zeros(tempRoi.size(), CV_8UC1)}; + cv::subtract(extremeCorners, + cv::Scalar{static_cast(boundingRect.tl().x), + static_cast(boundingRect.tl().y)}, + extremeCorners); + cv::Point2f tempCenter{}; + tempCenter.x = + std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0, + [extremeCorners](float acc, const cv::Point2f& p2) { + return acc + p2.x / extremeCorners.size(); + }); + tempCenter.y = + std::accumulate(extremeCorners.begin(), extremeCorners.end(), 0.0, + [extremeCorners](float acc, const cv::Point2f& p2) { + return acc + p2.y / extremeCorners.size(); + }); + + for (auto& corner : extremeCorners) { + float xDiff = corner.x - tempCenter.x; + float yDiff = corner.y - tempCenter.y; + xDiff += 1 * mathutil::sgn(xDiff); + yDiff += 1 * mathutil::sgn(yDiff); + corner = cv::Point2f{tempCenter.x + xDiff, tempCenter.y + yDiff}; + } + + std::vector extremeCornerInt{extremeCorners.begin(), + extremeCorners.end()}; + cv::fillConvexPoly(tempMask, extremeCornerInt, cv::Scalar{255}); + + cv::copyTo(tempRoi, destination(boundingRect), tempMask); +} + +static double GetScaledThickness(double thickness480p, + const cv::Mat& destinationMat) { + double scaleX = destinationMat.size().width / 640.0; + double scaleY = destinationMat.size().height / 480.0; + double minScale = std::min(scaleX, scaleY); + return std::max(thickness480p * minScale, 1.0); +} + +[[maybe_unused]] static void DrawInscribedEllipse( + const std::vector& dstPoints, const cv::Scalar& color, + cv::Mat& destination) { + cv::RotatedRect rect = OpenCVHelp::GetMinAreaRect(dstPoints); + cv::ellipse(destination, rect, color, -1, cv::LINE_AA); +} + +static void DrawPoly(const std::vector& dstPoints, int thickness, + const cv::Scalar& color, bool isClosed, + cv::Mat& destination) { + std::vector intDstPoints{dstPoints.begin(), dstPoints.end()}; + std::vector> listOfListOfPoints; + listOfListOfPoints.emplace_back(intDstPoints); + if (thickness > 0) { + cv::polylines(destination, listOfListOfPoints, isClosed, color, thickness, + cv::LINE_AA); + } else { + cv::fillPoly(destination, listOfListOfPoints, color, cv::LINE_AA); + } +} + +[[maybe_unused]] static void DrawTagDetection( + int id, const std::vector& dstPoints, cv::Mat& destination) { + double thickness = GetScaledThickness(1, destination); + DrawPoly(dstPoints, static_cast(thickness), cv::Scalar{0, 0, 255}, true, + destination); + cv::Rect2d rect{cv::boundingRect(dstPoints)}; + cv::Point2d textPt{rect.x + rect.width, rect.y}; + textPt.x += thickness; + textPt.y += thickness; + cv::putText(destination, std::to_string(id), textPt, cv::FONT_HERSHEY_PLAIN, + 1.5 * thickness, cv::Scalar{0, 200, 0}, + static_cast(thickness), cv::LINE_AA); +} + +static std::vector> GetFieldWallLines() { + std::vector> list; + + const units::meter_t sideHt = 19.5_in; + const units::meter_t driveHt = 35_in; + const units::meter_t topHt = 78_in; + + // field floor + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, 0_m}, + frc::Translation3d{fieldLength, 0_m, 0_m}, + frc::Translation3d{fieldLength, fieldWidth, 0_m}, + frc::Translation3d{0_m, fieldWidth, 0_m}, + frc::Translation3d{0_m, 0_m, 0_m}}); + + // right side wall + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Translation3d{0_m, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, 0_m}}); + + // red driverstation + list.emplace_back(std::vector{ + frc::Translation3d{fieldLength, 0_m, sideHt}, + frc::Translation3d{fieldLength, 0_m, topHt}, + frc::Translation3d{fieldLength, fieldWidth, topHt}, + frc::Translation3d{fieldLength, fieldWidth, sideHt}, + }); + list.emplace_back(std::vector{ + frc::Translation3d{fieldLength, 0_m, driveHt}, + frc::Translation3d{fieldLength, fieldWidth, driveHt}}); + + // left side wall + list.emplace_back(std::vector{ + frc::Translation3d{0_m, fieldWidth, 0_m}, + frc::Translation3d{0_m, fieldWidth, sideHt}, + frc::Translation3d{fieldLength, fieldWidth, sideHt}, + frc::Translation3d{fieldLength, fieldWidth, 0_m}}); + + // blue driverstation + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, sideHt}, + frc::Translation3d{0_m, 0_m, topHt}, + frc::Translation3d{0_m, fieldWidth, topHt}, + frc::Translation3d{0_m, fieldWidth, sideHt}, + }); + list.emplace_back(std::vector{ + frc::Translation3d{0_m, 0_m, driveHt}, + frc::Translation3d{0_m, fieldWidth, driveHt}}); + + return list; +} + +static std::vector> GetFieldFloorLines( + int subdivisions) { + std::vector> list; + const units::meter_t subLength = fieldLength / subdivisions; + const units::meter_t subWidth = fieldWidth / subdivisions; + + for (int i = 0; i < subdivisions; i++) { + list.emplace_back(std::vector{ + frc::Translation3d{0_m, subWidth * (i + 1), 0_m}, + frc::Translation3d{fieldLength, subWidth * (i + 1), 0_m}}); + list.emplace_back(std::vector{ + frc::Translation3d{subLength * (i + 1), 0_m, 0_m}, + frc::Translation3d{subLength * (i + 1), fieldWidth, 0_m}}); + } + return list; +} + +static std::vector> PolyFrom3dLines( + const RotTrlTransform3d& camRt, const SimCameraProperties& prop, + const std::vector& trls, double resolution, + bool isClosed, cv::Mat& destination) { + resolution = std::hypot(destination.size().height, destination.size().width) * + resolution; + std::vector pts{trls}; + if (isClosed) { + pts.emplace_back(pts[0]); + } + std::vector> polyPointList{}; + + for (size_t i = 0; i < pts.size() - 1; i++) { + frc::Translation3d pta = pts[i]; + frc::Translation3d ptb = pts[i + 1]; + + std::pair, std::optional> inter = + prop.GetVisibleLine(camRt, pta, ptb); + if (!inter.second) { + continue; + } + + double inter1 = inter.first.value(); + double inter2 = inter.second.value(); + frc::Translation3d baseDelta = ptb - pta; + frc::Translation3d old_pta = pta; + if (inter1 > 0) { + pta = old_pta + baseDelta * inter1; + } + if (inter2 < 1) { + ptb = old_pta + baseDelta * inter2; + } + baseDelta = ptb - pta; + + std::vector poly = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, {pta, ptb}); + cv::Point2d pxa = poly[0]; + cv::Point2d pxb = poly[1]; + + double pxDist = std::hypot(pxb.x - pxa.x, pxb.y - pxa.y); + int subdivisions = static_cast(pxDist / resolution); + frc::Translation3d subDelta = baseDelta / (subdivisions + 1); + std::vector subPts{}; + for (int j = 0; j < subdivisions; j++) { + subPts.emplace_back(pta + (subDelta * (j + 1))); + } + if (subPts.size() > 0) { + std::vector toAdd = OpenCVHelp::ProjectPoints( + prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, subPts); + poly.insert(poly.begin() + 1, toAdd.begin(), toAdd.end()); + } + + polyPointList.emplace_back(poly); + } + + return polyPointList; +} + +[[maybe_unused]] static void DrawFieldWireFrame( + const RotTrlTransform3d& camRt, const SimCameraProperties& prop, + double resolution, double wallThickness, const cv::Scalar& wallColor, + int floorSubdivisions, double floorThickness, const cv::Scalar& floorColor, + cv::Mat& destination) { + for (const auto& trls : GetFieldFloorLines(floorSubdivisions)) { + auto polys = + PolyFrom3dLines(camRt, prop, trls, resolution, false, destination); + for (const auto& poly : polys) { + DrawPoly(poly, + static_cast( + std::round(GetScaledThickness(floorThickness, destination))), + floorColor, false, destination); + } + } + for (const auto& trls : GetFieldWallLines()) { + auto polys = + PolyFrom3dLines(camRt, prop, trls, resolution, false, destination); + for (const auto& poly : polys) { + DrawPoly(poly, + static_cast( + std::round(GetScaledThickness(wallThickness, destination))), + wallColor, false, destination); + } + } +} +} // namespace VideoSimUtil +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h new file mode 100644 index 0000000000..55988bc32f --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h @@ -0,0 +1,282 @@ +/* + * MIT License + * + * Copyright (c) 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 +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "photon/simulation/PhotonCameraSim.h" + +namespace photon { +class VisionSystemSim { + public: + explicit VisionSystemSim(std::string visionSystemName) { + std::string tableName = "VisionSystemSim-" + visionSystemName; + frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField); + } + std::optional GetCameraSim(std::string name) { + auto it = camSimMap.find(name); + if (it != camSimMap.end()) { + return std::make_optional(it->second); + } else { + return std::nullopt; + } + } + std::vector GetCameraSims() { + std::vector retVal; + for (auto const& cam : camSimMap) { + retVal.emplace_back(cam.second); + } + return retVal; + } + void AddCamera(PhotonCameraSim* cameraSim, + const frc::Transform3d& robotToCamera) { + auto found = + camSimMap.find(std::string{cameraSim->GetCamera()->GetCameraName()}); + if (found == camSimMap.end()) { + camSimMap[std::string{cameraSim->GetCamera()->GetCameraName()}] = + cameraSim; + camTrfMap.insert(std::make_pair( + std::move(cameraSim), + frc::TimeInterpolatableBuffer{bufferLength})); + camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(), + frc::Pose3d{} + robotToCamera); + } + } + void ClearCameras() { + camSimMap.clear(); + camTrfMap.clear(); + } + bool RemoveCamera(PhotonCameraSim* cameraSim) { + int numOfElementsRemoved = + camSimMap.erase(std::string{cameraSim->GetCamera()->GetCameraName()}); + if (numOfElementsRemoved == 1) { + return true; + } else { + return false; + } + } + std::optional GetRobotToCamera(PhotonCameraSim* cameraSim) { + return GetRobotToCamera(cameraSim, frc::Timer::GetFPGATimestamp()); + } + std::optional GetRobotToCamera(PhotonCameraSim* cameraSim, + units::second_t time) { + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + frc::TimeInterpolatableBuffer trfBuffer = + camTrfMap.at(cameraSim); + std::optional sample = trfBuffer.Sample(time); + if (!sample) { + return std::nullopt; + } else { + return std::make_optional( + frc::Transform3d{frc::Pose3d{}, sample.value_or(frc::Pose3d{})}); + } + } else { + return std::nullopt; + } + } + std::optional GetCameraPose(PhotonCameraSim* cameraSim) { + return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp()); + } + std::optional GetCameraPose(PhotonCameraSim* cameraSim, + units::second_t time) { + auto robotToCamera = GetRobotToCamera(cameraSim, time); + if (!robotToCamera) { + return std::nullopt; + } else { + return std::make_optional(GetRobotPose(time) + robotToCamera.value()); + } + } + bool AdjustCamera(PhotonCameraSim* cameraSim, + const frc::Transform3d& robotToCamera) { + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(), + frc::Pose3d{} + robotToCamera); + return true; + } else { + return false; + } + } + void ResetCameraTransforms() { + for (const auto& pair : camTrfMap) { + ResetCameraTransforms(pair.first); + } + } + bool ResetCameraTransforms(PhotonCameraSim* cameraSim) { + units::second_t now = frc::Timer::GetFPGATimestamp(); + if (camTrfMap.find(cameraSim) != camTrfMap.end()) { + auto trfBuffer = camTrfMap.at(cameraSim); + frc::Transform3d lastTrf{frc::Pose3d{}, + trfBuffer.Sample(now).value_or(frc::Pose3d{})}; + trfBuffer.Clear(); + AdjustCamera(cameraSim, lastTrf); + return true; + } else { + return false; + } + } + std::vector GetVisionTargets() { + std::vector all{}; + for (const auto& entry : targetSets) { + for (const auto& target : entry.second) { + all.emplace_back(target); + } + } + return all; + } + std::vector GetVisionTargets(std::string type) { + return targetSets[type]; + } + void AddVisionTargets(const std::vector& targets) { + AddVisionTargets("targets", targets); + } + void AddVisionTargets(std::string type, + const std::vector& targets) { + if (!targetSets.contains(type)) { + targetSets[type] = std::vector{}; + } + for (const auto& tgt : targets) { + targetSets[type].emplace_back(tgt); + } + } + void AddAprilTags(const frc::AprilTagFieldLayout& layout) { + std::vector targets; + for (const frc::AprilTag& tag : layout.GetTags()) { + targets.emplace_back(VisionTargetSim{layout.GetTagPose(tag.ID).value(), + photon::kAprilTag16h5, tag.ID}); + } + AddVisionTargets("apriltag", targets); + } + void ClearVisionTargets() { targetSets.clear(); } + void ClearAprilTags() { RemoveVisionTargets("apriltag"); } + void RemoveVisionTargets(std::string type) { targetSets.erase(type); } + std::vector RemoveVisionTargets( + const std::vector& targets) { + std::vector removedList; + for (auto& entry : targetSets) { + for (auto target : entry.second) { + auto it = std::find(targets.begin(), targets.end(), target); + if (it != targets.end()) { + removedList.emplace_back(target); + entry.second.erase(it); + } + } + } + return removedList; + } + frc::Pose3d GetRobotPose() { + return GetRobotPose(frc::Timer::GetFPGATimestamp()); + } + frc::Pose3d GetRobotPose(units::second_t timestamp) { + return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{}); + } + void ResetRobotPose(const frc::Pose2d& robotPose) { + ResetRobotPose(frc::Pose3d{robotPose}); + } + void ResetRobotPose(const frc::Pose3d& robotPose) { + robotPoseBuffer.Clear(); + robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose); + } + frc::Field2d& GetDebugField() { return dbgField; } + void Update(const frc::Pose2d& robotPose) { Update(frc::Pose3d{robotPose}); } + void Update(const frc::Pose3d& robotPose) { + for (auto& set : targetSets) { + std::vector posesToAdd{}; + for (auto& target : set.second) { + posesToAdd.emplace_back(target.GetPose().ToPose2d()); + } + dbgField.GetObject(set.first)->SetPoses(posesToAdd); + } + + units::second_t now = frc::Timer::GetFPGATimestamp(); + robotPoseBuffer.AddSample(now, robotPose); + dbgField.SetRobotPose(robotPose.ToPose2d()); + + std::vector allTargets{}; + for (const auto& set : targetSets) { + for (const auto& target : set.second) { + allTargets.emplace_back(target); + } + } + + std::vector visTgtPoses2d{}; + std::vector cameraPoses2d{}; + bool processed{false}; + for (const auto& entry : camSimMap) { + auto camSim = entry.second; + auto optTimestamp = camSim->ConsumeNextEntryTime(); + if (!optTimestamp) { + continue; + } else { + processed = true; + } + uint64_t timestampNt = optTimestamp.value(); + units::second_t latency = camSim->prop.EstLatency(); + units::second_t timestampCapture = + units::microsecond_t{static_cast(timestampNt)} - latency; + + frc::Pose3d lateRobotPose = GetRobotPose(timestampCapture); + frc::Pose3d lateCameraPose = + lateRobotPose + GetRobotToCamera(camSim, timestampCapture).value(); + cameraPoses2d.push_back(lateCameraPose.ToPose2d()); + + auto camResult = camSim->Process(latency, lateCameraPose, allTargets); + camSim->SubmitProcessedFrame(camResult, timestampNt); + for (const auto& target : camResult.GetTargets()) { + auto trf = target.GetBestCameraToTarget(); + if (trf == kEmptyTrf) { + continue; + } + visTgtPoses2d.push_back(lateCameraPose.TransformBy(trf).ToPose2d()); + } + } + if (processed) { + dbgField.GetObject("visibleTargetPoses")->SetPoses(visTgtPoses2d); + } + if (cameraPoses2d.size() != 0) { + dbgField.GetObject("cameras")->SetPoses(cameraPoses2d); + } + } + + private: + std::unordered_map camSimMap{}; + static constexpr units::second_t bufferLength{1.5_s}; + std::unordered_map> + camTrfMap; + frc::TimeInterpolatableBuffer robotPoseBuffer{bufferLength}; + std::unordered_map> targetSets{}; + frc::Field2d dbgField{}; + const frc::Transform3d kEmptyTrf{}; +}; +} // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h new file mode 100644 index 0000000000..f8d4d6676b --- /dev/null +++ b/photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h @@ -0,0 +1,73 @@ +/* + * MIT License + * + * Copyright (c) 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 + +#include + +#include "photon/estimation/TargetModel.h" + +namespace photon { +class VisionTargetSim { + public: + VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model) + : fiducialId(-1), pose(pose), model(model) {} + VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model, int id) + : fiducialId(id), pose(pose), model(model) {} + void SetPose(const frc::Pose3d& newPose) { pose = newPose; } + void SetModel(const TargetModel& newModel) { model = newModel; } + frc::Pose3d GetPose() const { return pose; } + TargetModel GetModel() const { return model; } + std::vector GetFieldVertices() const { + return model.GetFieldVertices(pose); + } + int fiducialId; + + bool operator<(const VisionTargetSim& right) const { + return pose.Translation().Norm() < right.pose.Translation().Norm(); + } + + bool operator==(const VisionTargetSim& other) const { + return units::math::abs(pose.Translation().X() - + other.GetPose().Translation().X()) < 1_in && + units::math::abs(pose.Translation().Y() - + other.GetPose().Translation().Y()) < 1_in && + units::math::abs(pose.Translation().Z() - + other.GetPose().Translation().Z()) < 1_in && + units::math::abs(pose.Rotation().X() - + other.GetPose().Rotation().X()) < 1_deg && + units::math::abs(pose.Rotation().Y() - + other.GetPose().Rotation().Y()) < 1_deg && + units::math::abs(pose.Rotation().Z() - + other.GetPose().Rotation().Z()) < 1_deg && + model.GetIsPlanar() == other.GetModel().GetIsPlanar(); + } + + private: + frc::Pose3d pose; + TargetModel model; +}; +} // namespace photon diff --git a/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp new file mode 100644 index 0000000000..1c3f9eac58 --- /dev/null +++ b/photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp @@ -0,0 +1,461 @@ +/* + * MIT License + * + * Copyright (c) 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 "gtest/gtest.h" +#include "photon/PhotonUtils.h" +#include "photon/simulation/VisionSystemSim.h" + +class VisionSystemSimTest : public ::testing::Test { + void SetUp() override { + nt::NetworkTableInstance::GetDefault().StartServer(); + photon::PhotonCamera::SetVersionCheckEnabled(false); + } + + void TearDown() override {} +}; + +class VisionSystemSimTestWithParamsTest + : public VisionSystemSimTest, + public testing::WithParamInterface {}; +class VisionSystemSimTestDistanceParamsTest + : public VisionSystemSimTest, + public testing::WithParamInterface< + std::tuple> {}; + +TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{1.0_m, 1.0_m}, 3}}); + + // To the right, to the right + frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-70_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the right, to the right + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-95_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{90_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // To the left, to the left + robotPose = + frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{65_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // Now kick, now kick + robotPose = frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + // Now kick, now kick + robotPose = + frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + // Now walk it by yourself + robotPose = + frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-179_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); + + // Now walk it by yourself + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleVert1) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{3.0_m, 3.0_m}, 3}}); + + frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 5000_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}}); + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleVert2) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 2_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + frc::Transform3d robotToCamera{ + frc::Translation3d{0_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, units::radian_t{-std::numbers::pi / 4}, 0_rad}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, robotToCamera); + cameraSim.prop.SetCalibration(1234, 1234, frc::Rotation2d{80_deg}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 1736}}); + + frc::Pose2d robotPose{frc::Translation2d{13.98_m, 0_m}, + frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.1_m, 0.1_m}, 24}}); + + frc::Pose2d robotPose{frc::Translation2d{12_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) { + frc::Pose3d targetPose{ + frc::Translation3d{15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(1.0); + cameraSim.SetMaxSightRange(10_m); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{targetPose, photon::TargetModel{1_m, 1_m}, 25}}); + + frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + + robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}}; + visionSysSim.Update(robotPose); + ASSERT_FALSE(camera.GetLatestResult().HasTargets()); +} + +TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + + robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m}, + frc::Rotation2d{-1 * GetParam()}}; + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + ASSERT_NEAR(GetParam().to(), + camera.GetLatestResult().GetBestTarget().GetYaw(), 0.25); +} + +TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) { + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}}; + frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}}; + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{120_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}}); + + robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m}, + frc::Rotation2d{-1 * GetParam()}}; + visionSysSim.AdjustCamera( + &cameraSim, + frc::Transform3d{ + frc::Translation3d{}, + frc::Rotation3d{0_rad, units::degree_t{GetParam()}, 0_rad}}); + visionSysSim.Update(robotPose); + ASSERT_TRUE(camera.GetLatestResult().HasTargets()); + ASSERT_NEAR(GetParam().to(), + camera.GetLatestResult().GetBestTarget().GetPitch(), 0.25); +} + +INSTANTIATE_TEST_SUITE_P(AnglesTests, VisionSystemSimTestWithParamsTest, + testing::Values(-10_deg, -5_deg, -0_deg, -1_deg, + -2_deg, 5_deg, 7_deg, 10.23_deg)); + +TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) { + units::foot_t distParam; + units::degree_t pitchParam; + units::foot_t heightParam; + std::tie(distParam, pitchParam, heightParam) = GetParam(); + + const frc::Pose3d targetPose{ + {15.98_m, 0_m, 1_m}, + frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi * 0.98}}}; + frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}}; + frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam}, + frc::Rotation3d{0_deg, pitchParam, 0_deg}}; + photon::VisionSystemSim visionSysSim{ + "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho" + "wsyourdaygoingihopegoodhaveagreatrestofyourlife"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{160_deg}); + cameraSim.SetMinTargetAreaPixels(0.0); + visionSysSim.AdjustCamera(&cameraSim, robotToCamera); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPose, photon::TargetModel{0.5_m, 0.5_m}, 0}}); + visionSysSim.Update(robotPose); + + photon::PhotonPipelineResult res = camera.GetLatestResult(); + ASSERT_TRUE(res.HasTargets()); + photon::PhotonTrackedTarget target = res.GetBestTarget(); + + ASSERT_NEAR(0.0, target.GetYaw(), 0.5); + + units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget( + robotToCamera.Z(), targetPose.Z(), -pitchParam, + units::degree_t{target.GetPitch()}); + ASSERT_NEAR(dist.to(), + distParam.convert().to(), 0.25); +} + +INSTANTIATE_TEST_SUITE_P( + DistanceParamsTests, VisionSystemSimTestDistanceParamsTest, + testing::Values(std::make_tuple(5_ft, -15.98_deg, 0_ft), + std::make_tuple(6_ft, -15.98_deg, 1_ft), + std::make_tuple(10_ft, -15.98_deg, 0_ft), + std::make_tuple(15_ft, -15.98_deg, 2_ft), + std::make_tuple(19.95_ft, -15.98_deg, 0_ft), + std::make_tuple(20_ft, -15.98_deg, 0_ft), + std::make_tuple(5_ft, -42_deg, 1_ft), + std::make_tuple(6_ft, -42_deg, 0_ft), + std::make_tuple(10_ft, -42_deg, 2_ft), + std::make_tuple(15_ft, -42_deg, 0.5_ft), + std::make_tuple(19.42_ft, -15.98_deg, 0_ft), + std::make_tuple(20_ft, -42_deg, 0_ft), + std::make_tuple(5_ft, -55_deg, 2_ft), + std::make_tuple(6_ft, -55_deg, 0_ft), + std::make_tuple(10_ft, -54_deg, 2.2_ft), + std::make_tuple(15_ft, -53_deg, 0_ft), + std::make_tuple(19.52_ft, -15.98_deg, 1.1_ft))); + +TEST_F(VisionSystemSimTest, TestMultipleTargets) { + frc::Pose3d targetPoseL{ + frc::Translation3d{15.98_m, 2_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + frc::Pose3d targetPoseC{ + frc::Translation3d{15.98_m, 0_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + frc::Pose3d targetPoseR{ + frc::Translation3d{15.98_m, -2_m, 0_m}, + frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}}; + + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 1}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 2}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 3}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 4}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 5}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 6}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 7}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseC.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 8}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 9}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseR.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 10}}); + visionSysSim.AddVisionTargets({photon::VisionTargetSim{ + targetPoseL.TransformBy(frc::Transform3d{ + frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}), + photon::kAprilTag16h5, 11}}); + + frc::Pose2d robotPose{frc::Translation2d{6_m, 0_m}, frc::Rotation2d{.25_deg}}; + visionSysSim.Update(robotPose); + photon::PhotonPipelineResult res = camera.GetLatestResult(); + ASSERT_TRUE(res.HasTargets()); + std::span tgtList = res.GetTargets(); + ASSERT_EQ(static_cast(11), tgtList.size()); +} + +TEST_F(VisionSystemSimTest, TestPoseEstimation) { + photon::VisionSystemSim visionSysSim{"Test"}; + photon::PhotonCamera camera{"camera"}; + photon::PhotonCameraSim cameraSim{&camera}; + visionSysSim.AddCamera(&cameraSim, frc::Transform3d{}); + cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg}); + cameraSim.SetMinTargetAreaPixels(20.0); + + std::vector tagList; + tagList.emplace_back(frc::AprilTag{ + 0, frc::Pose3d{12_m, 3_m, 1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 1, frc::Pose3d{12_m, 1_m, -1_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + tagList.emplace_back(frc::AprilTag{ + 2, frc::Pose3d{11_m, 0_m, 2_m, + frc::Rotation3d{0_rad, 0_rad, + units::radian_t{std::numbers::pi}}}}); + units::meter_t fieldLength{54}; + units::meter_t fieldWidth{27}; + frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth}; + frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{5_deg}}; + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag16h5, 0}}); + visionSysSim.Update(robotPose); + + Eigen::Matrix camEigen; + cv::cv2eigen(camera.GetCameraMatrix().value(), camEigen); + + Eigen::Matrix distEigen; + cv::cv2eigen(camera.GetDistCoeffs().value(), distEigen); + + auto camResults = camera.GetLatestResult(); + auto targetSpan = camResults.GetTargets(); + std::vector targets; + for (photon::PhotonTrackedTarget tar : targetSpan) { + targets.push_back(tar); + } + photon::PNPResult results = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets, layout, photon::kAprilTag16h5); + frc::Pose3d pose = frc::Pose3d{} + results.best; + ASSERT_NEAR(5, pose.X().to(), 0.01); + ASSERT_NEAR(1, pose.Y().to(), 0.01); + ASSERT_NEAR(0, pose.Z().to(), 0.01); + ASSERT_NEAR(units::degree_t{5}.convert().to(), + pose.Rotation().Z().to(), 0.01); + + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[1].pose, photon::kAprilTag16h5, 1}}); + visionSysSim.AddVisionTargets( + {photon::VisionTargetSim{tagList[2].pose, photon::kAprilTag16h5, 2}}); + visionSysSim.Update(robotPose); + + auto camResults2 = camera.GetLatestResult(); + auto targetSpan2 = camResults2.GetTargets(); + std::vector targets2; + for (photon::PhotonTrackedTarget tar : targetSpan2) { + targets2.push_back(tar); + } + photon::PNPResult results2 = photon::VisionEstimation::EstimateCamPosePNP( + camEigen, distEigen, targets2, layout, photon::kAprilTag16h5); + frc::Pose3d pose2 = frc::Pose3d{} + results2.best; + ASSERT_NEAR(5, pose2.X().to(), 0.01); + ASSERT_NEAR(1, pose2.Y().to(), 0.01); + ASSERT_NEAR(0, pose2.Z().to(), 0.01); + ASSERT_NEAR(units::degree_t{5}.convert().to(), + pose2.Rotation().Z().to(), 0.01); +} diff --git a/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h new file mode 100644 index 0000000000..a0a0fb0e61 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/CameraTargetRelation.h @@ -0,0 +1,61 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +namespace photon { +class CameraTargetRelation { + public: + const frc::Pose3d camPose; + const frc::Transform3d camToTarg; + const units::meter_t camToTargDist; + const units::meter_t camToTargDistXY; + const frc::Rotation2d camToTargYaw; + const frc::Rotation2d camToTargPitch; + + const frc::Rotation2d camToTargAngle; + + const frc::Transform3d targToCam; + const frc::Rotation2d targToCamYaw; + const frc::Rotation2d targToCamPitch; + + const frc::Rotation2d targToCamAngle; + + CameraTargetRelation(const frc::Pose3d& cameraPose, + const frc::Pose3d& targetPose) + : camPose(cameraPose), + camToTarg(frc::Transform3d{cameraPose, targetPose}), + camToTargDist(camToTarg.Translation().Norm()), + camToTargDistXY(units::math::hypot(camToTarg.Translation().X(), + camToTarg.Translation().Y())), + camToTargYaw(frc::Rotation2d{camToTarg.X().to(), + camToTarg.Y().to()}), + camToTargPitch(frc::Rotation2d{camToTargDistXY.to(), + -camToTarg.Z().to()}), + camToTargAngle(frc::Rotation2d{units::math::hypot( + camToTargYaw.Radians(), camToTargPitch.Radians())}), + targToCam(frc::Transform3d{targetPose, cameraPose}), + targToCamYaw(frc::Rotation2d{targToCam.X().to(), + targToCam.Y().to()}), + targToCamPitch(frc::Rotation2d{camToTargDistXY.to(), + -targToCam.Z().to()}), + targToCamAngle(frc::Rotation2d{units::math::hypot( + targToCamYaw.Radians(), targToCamPitch.Radians())}) {} +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h new file mode 100644 index 0000000000..2d79e661e0 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h @@ -0,0 +1,291 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "RotTrlTransform3d.h" + +#define OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT +#include +#include "photon/targeting/PNPResult.h" +#include "photon/targeting/MultiTargetPNPResult.h" + +namespace photon { +namespace OpenCVHelp { + +static frc::Rotation3d NWU_TO_EDN{ + (Eigen::Matrix3d() << 0, -1, 0, 0, 0, -1, 1, 0, 0).finished()}; +static frc::Rotation3d EDN_TO_NWU{ + (Eigen::Matrix3d() << 0, 0, 1, -1, 0, 0, 0, -1, 0).finished()}; + +static std::vector GetConvexHull( + const std::vector& points) { + std::vector outputHull{}; + cv::convexHull(points, outputHull); + std::vector convexPoints; + for (size_t i = 0; i < outputHull.size(); i++) { + convexPoints.push_back(points[outputHull[i]]); + } + return convexPoints; +} + +static cv::RotatedRect GetMinAreaRect(const std::vector& points) { + return cv::minAreaRect(points); +} + +static frc::Translation3d TranslationNWUtoEDN(const frc::Translation3d& trl) { + return trl.RotateBy(NWU_TO_EDN); +} + +static frc::Rotation3d RotationNWUtoEDN(const frc::Rotation3d& rot) { + return -NWU_TO_EDN + (rot + NWU_TO_EDN); +} + +static std::vector TranslationToTVec( + const std::vector& translations) { + std::vector retVal; + retVal.reserve(translations.size()); + for (size_t i = 0; i < translations.size(); i++) { + frc::Translation3d trl = TranslationNWUtoEDN(translations[i]); + retVal.emplace_back(cv::Point3f{trl.X().to(), trl.Y().to(), + trl.Z().to()}); + } + return retVal; +} + +static std::vector RotationToRVec( + const frc::Rotation3d& rotation) { + std::vector retVal{}; + frc::Rotation3d rot = RotationNWUtoEDN(rotation); + retVal.emplace_back(cv::Point3d{ + rot.GetQuaternion().ToRotationVector()(0), + rot.GetQuaternion().ToRotationVector()(1), + rot.GetQuaternion().ToRotationVector()(2), + }); + return retVal; +} + +[[maybe_unused]] static cv::Point2f AvgPoint(std::vector points) { + if (points.size() == 0) { + return cv::Point2f{}; + } + cv::reduce(points, points, 0, cv::REDUCE_AVG); + return points[0]; +} + +[[maybe_unused]] static std::vector> PointsToCorners( + const std::vector& points) { + std::vector> retVal; + retVal.reserve(points.size()); + for (size_t i = 0; i < points.size(); i++) { + retVal.emplace_back(std::make_pair(points[i].x, points[i].y)); + } + return retVal; +} + +[[maybe_unused]] static std::vector CornersToPoints( + const std::vector>& corners) { + std::vector retVal; + retVal.reserve(corners.size()); + for (size_t i = 0; i < corners.size(); i++) { + retVal.emplace_back(cv::Point2f{corners[i].first, corners[i].second}); + } + return retVal; +} + +[[maybe_unused]] static cv::Rect GetBoundingRect( + const std::vector& points) { + return cv::boundingRect(points); +} + +static std::vector ProjectPoints( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + const RotTrlTransform3d& camRt, + const std::vector& objectTranslations) { + std::vector objectPoints = TranslationToTVec(objectTranslations); + std::vector rvec = RotationToRVec(camRt.GetRotation()); + std::vector tvec = TranslationToTVec({camRt.GetTranslation()}); + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + std::vector imagePoints{}; + cv::projectPoints(objectPoints, rvec, tvec, cameraMat, distCoeffsMat, + imagePoints); + return imagePoints; +} + +template +static std::vector ReorderCircular(const std::vector elements, + bool backwards, int shiftStart) { + size_t size = elements.size(); + int dir = backwards ? -1 : 1; + std::vector reordered{elements}; + for (size_t i = 0; i < size; i++) { + int index = (i * dir + shiftStart * dir) % size; + if (index < 0) { + index = size + index; + } + reordered[i] = elements[index]; + } + return reordered; +} + +static frc::Translation3d TranslationEDNToNWU(const frc::Translation3d& trl) { + return trl.RotateBy(EDN_TO_NWU); +} + +static frc::Rotation3d RotationEDNToNWU(const frc::Rotation3d& rot) { + return -EDN_TO_NWU + (rot + EDN_TO_NWU); +} + +static frc::Translation3d TVecToTranslation(const cv::Mat& tvecInput) { + cv::Vec3f data{}; + cv::Mat wrapped{tvecInput.rows, tvecInput.cols, CV_32F}; + tvecInput.convertTo(wrapped, CV_32F); + data = wrapped.at(cv::Point{0, 0}); + return TranslationEDNToNWU(frc::Translation3d{units::meter_t{data[0]}, + units::meter_t{data[1]}, + units::meter_t{data[2]}}); +} + +static frc::Rotation3d RVecToRotation(const cv::Mat& rvecInput) { + cv::Vec3f data{}; + cv::Mat wrapped{rvecInput.rows, rvecInput.cols, CV_32F}; + rvecInput.convertTo(wrapped, CV_32F); + data = wrapped.at(cv::Point{0, 0}); + return RotationEDNToNWU(frc::Rotation3d{units::radian_t{data[0]}, + units::radian_t{data[1]}, + units::radian_t{data[2]}}); +} + +[[maybe_unused]] static photon::PNPResult SolvePNP_Square( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + std::vector modelTrls, + std::vector imagePoints) { + modelTrls = ReorderCircular(modelTrls, true, -1); + imagePoints = ReorderCircular(imagePoints, true, -1); + std::vector objectMat = TranslationToTVec(modelTrls); + std::vector rvecs; + std::vector tvecs; + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F); + + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_32F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_32F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + + cv::Vec2d errors{}; + frc::Transform3d best{}; + std::optional alt{std::nullopt}; + + for (int tries = 0; tries < 2; tries++) { + cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs, + tvecs, false, cv::SOLVEPNP_IPPE_SQUARE, rvec, tvec, + reprojectionError); + + errors = reprojectionError.at(cv::Point{0, 0}); + best = frc::Transform3d{TVecToTranslation(tvecs.at(0)), + RVecToRotation(rvecs[0])}; + + if (tvecs.size() > 1) { + alt = frc::Transform3d{TVecToTranslation(tvecs.at(1)), + RVecToRotation(rvecs[1])}; + } + + if (!std::isnan(errors[0])) { + break; + } else { + cv::Point2f pt = imagePoints[0]; + pt.x -= 0.001f; + pt.y -= 0.001f; + imagePoints[0] = pt; + } + } + + if (std::isnan(errors[0])) { + fmt::print("SolvePNP_Square failed!\n"); + } + if (alt) { + photon::PNPResult result; + result.best = best; + result.alt = alt.value(); + result.ambiguity = errors[0] / errors[1]; + result.bestReprojErr = errors[0]; + result.altReprojErr = errors[1]; + result.isPresent = true; + return result; + } else { + photon::PNPResult result; + result.best = best; + result.bestReprojErr = errors[0]; + result.isPresent = true; + return result; + } +} + +[[maybe_unused]] static photon::PNPResult SolvePNP_SQPNP( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + std::vector modelTrls, + std::vector imagePoints) { + std::vector objectMat = TranslationToTVec(modelTrls); + std::vector rvecs{}; + std::vector tvecs{}; + cv::Mat rvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat tvec = cv::Mat::zeros(3, 1, CV_32F); + cv::Mat reprojectionError = cv::Mat::zeros(2, 1, CV_32F); + + cv::Mat cameraMat(cameraMatrix.rows(), cameraMatrix.cols(), CV_64F); + cv::eigen2cv(cameraMatrix, cameraMat); + cv::Mat distCoeffsMat(distCoeffs.rows(), distCoeffs.cols(), CV_64F); + cv::eigen2cv(distCoeffs, distCoeffsMat); + + float error = 0; + frc::Transform3d best{}; + + cv::solvePnPGeneric(objectMat, imagePoints, cameraMat, distCoeffsMat, rvecs, + tvecs, false, cv::SOLVEPNP_SQPNP, rvec, tvec, + reprojectionError); + + error = reprojectionError.at(cv::Point{0, 0}); + best = frc::Transform3d{TVecToTranslation(tvecs.at(0)), + RVecToRotation(rvecs[0])}; + + if (std::isnan(error)) { + fmt::print("SolvePNP_Square failed!\n"); + } + photon::PNPResult result; + result.best = best; + result.bestReprojErr = error; + result.isPresent = true; + return result; +} +} // namespace OpenCVHelp +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h new file mode 100644 index 0000000000..99e19bf2c3 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/RotTrlTransform3d.h @@ -0,0 +1,102 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +#include +#include +#include + +namespace photon { +class RotTrlTransform3d { + public: + RotTrlTransform3d(const frc::Rotation3d& rot, const frc::Translation3d& trl) + : trl(trl), rot(rot) {} + RotTrlTransform3d(const frc::Pose3d& initial, const frc::Pose3d& last) + : trl(last.Translation() - initial.Translation().RotateBy(rot)), + rot(last.Rotation() - initial.Rotation()) {} + explicit RotTrlTransform3d(const frc::Transform3d& trf) + : RotTrlTransform3d(trf.Rotation(), trf.Translation()) {} + RotTrlTransform3d() + : RotTrlTransform3d(frc::Rotation3d{}, frc::Translation3d{}) {} + + static RotTrlTransform3d MakeRelativeTo(const frc::Pose3d& pose) { + return RotTrlTransform3d{pose.Rotation(), pose.Translation()}.Inverse(); + } + + RotTrlTransform3d Inverse() const { + frc::Rotation3d invRot = -rot; + frc::Translation3d invTrl = -(trl.RotateBy(invRot)); + return RotTrlTransform3d{invRot, invTrl}; + } + + frc::Transform3d GetTransform() const { return frc::Transform3d{trl, rot}; } + + frc::Translation3d GetTranslation() const { return trl; } + + frc::Rotation3d GetRotation() const { return rot; } + + frc::Translation3d Apply(const frc::Translation3d& trlToApply) const { + return trlToApply.RotateBy(rot) + trl; + } + + std::vector ApplyTrls( + const std::vector& trls) const { + std::vector retVal; + retVal.reserve(trls.size()); + for (const auto& currentTrl : trls) { + retVal.emplace_back(Apply(currentTrl)); + } + return retVal; + } + + frc::Rotation3d Apply(const frc::Rotation3d& rotToApply) const { + return rotToApply + rot; + } + + std::vector ApplyTrls( + const std::vector& rots) const { + std::vector retVal; + retVal.reserve(rots.size()); + for (const auto& currentRot : rots) { + retVal.emplace_back(Apply(currentRot)); + } + return retVal; + } + + frc::Pose3d Apply(const frc::Pose3d& poseToApply) const { + return frc::Pose3d{Apply(poseToApply.Translation()), + Apply(poseToApply.Rotation())}; + } + + std::vector ApplyPoses( + const std::vector& poses) const { + std::vector retVal; + retVal.reserve(poses.size()); + for (const auto& currentPose : poses) { + retVal.emplace_back(Apply(currentPose)); + } + return retVal; + } + + private: + const frc::Translation3d trl; + const frc::Rotation3d rot; +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h new file mode 100644 index 0000000000..64392fdb11 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/TargetModel.h @@ -0,0 +1,115 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +#include +#include + +#include "RotTrlTransform3d.h" + +namespace photon { +class TargetModel { + public: + TargetModel(units::meter_t width, units::meter_t height) + : vertices({frc::Translation3d{0_m, -width / 2.0, -height / 2.0}, + frc::Translation3d{0_m, width / 2.0, -height / 2.0}, + frc::Translation3d{0_m, width / 2.0, height / 2.0}, + frc::Translation3d{0_m, -width / 2.0, height / 2.0}}), + isPlanar(true), + isSpherical(false) {} + + TargetModel(units::meter_t length, units::meter_t width, + units::meter_t height) + : TargetModel({ + frc::Translation3d{length / 2.0, -width / 2.0, -height / 2.0}, + frc::Translation3d{length / 2.0, width / 2.0, -height / 2.0}, + frc::Translation3d{length / 2.0, width / 2.0, height / 2.0}, + frc::Translation3d{length / 2.0, -width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, -width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, width / 2.0, height / 2.0}, + frc::Translation3d{-length / 2.0, width / 2.0, -height / 2.0}, + frc::Translation3d{-length / 2.0, -width / 2.0, -height / 2.0}, + }) {} + + explicit TargetModel(units::meter_t diameter) + : vertices({ + frc::Translation3d{0_m, -diameter / 2.0, 0_m}, + frc::Translation3d{0_m, 0_m, -diameter / 2.0}, + frc::Translation3d{0_m, diameter / 2.0, 0_m}, + frc::Translation3d{0_m, 0_m, diameter / 2.0}, + }), + isPlanar(false), + isSpherical(true) {} + + explicit TargetModel(const std::vector& verts) + : isSpherical(false) { + if (verts.size() <= 2) { + vertices = std::vector(); + isPlanar = false; + } else { + bool cornersPlanar = true; + for (const auto& corner : verts) { + if (corner.X() != 0_m) { + cornersPlanar = false; + } + } + isPlanar = cornersPlanar; + } + vertices = verts; + } + + std::vector GetFieldVertices( + const frc::Pose3d& targetPose) const { + RotTrlTransform3d basisChange{targetPose.Rotation(), + targetPose.Translation()}; + std::vector retVal; + retVal.reserve(vertices.size()); + for (const auto& vert : vertices) { + retVal.emplace_back(basisChange.Apply(vert)); + } + return retVal; + } + + static frc::Pose3d GetOrientedPose(const frc::Translation3d& tgtTrl, + const frc::Translation3d& cameraTrl) { + frc::Translation3d relCam = cameraTrl - tgtTrl; + frc::Rotation3d orientToCam = frc::Rotation3d{ + 0_rad, + frc::Rotation2d{units::math::hypot(relCam.X(), relCam.Y()).to(), + -relCam.Z().to()} + .Radians(), + frc::Rotation2d{relCam.X().to(), relCam.Y().to()} + .Radians()}; + return frc::Pose3d{tgtTrl, orientToCam}; + } + + std::vector GetVertices() const { return vertices; } + bool GetIsPlanar() const { return isPlanar; } + bool GetIsSpherical() const { return isSpherical; } + + private: + std::vector vertices; + bool isPlanar; + bool isSpherical; +}; + +static const TargetModel kAprilTag16h5{6_in, 6_in}; +static const TargetModel kAprilTag36h11{6.5_in, 6.5_in}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h new file mode 100644 index 0000000000..cb3ae3bdc5 --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h @@ -0,0 +1,121 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include +#include + +#include +#include +#include + +#include "OpenCVHelp.h" +#include "TargetModel.h" +#include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" + +namespace photon { +namespace VisionEstimation { + +static std::vector GetVisibleLayoutTags( + const std::vector& visTags, + const frc::AprilTagFieldLayout& layout) { + std::vector retVal{}; + for (const auto& tag : visTags) { + int id = tag.GetFiducialId(); + auto maybePose = layout.GetTagPose(id); + if (maybePose) { + retVal.emplace_back(frc::AprilTag{id, maybePose.value()}); + } + } + return retVal; +} + +static PNPResult EstimateCamPosePNP( + const Eigen::Matrix& cameraMatrix, + const Eigen::Matrix& distCoeffs, + const std::vector& visTags, + const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) { + if (visTags.size() == 0) { + return PNPResult(); + } + + std::vector> corners{}; + std::vector knownTags{}; + + for (const auto& tgt : visTags) { + int id = tgt.GetFiducialId(); + auto maybePose = layout.GetTagPose(id); + if (maybePose) { + knownTags.emplace_back(frc::AprilTag{id, maybePose.value()}); + auto currentCorners = tgt.GetDetectedCorners(); + corners.insert(corners.end(), currentCorners.begin(), + currentCorners.end()); + } + } + if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { + return PNPResult{}; + } + + std::vector points = OpenCVHelp::CornersToPoints(corners); + + if (knownTags.size() == 1) { + PNPResult camToTag = OpenCVHelp::SolvePNP_Square( + cameraMatrix, distCoeffs, tagModel.GetVertices(), points); + if (!camToTag.isPresent) { + return PNPResult{}; + } + frc::Pose3d bestPose = + knownTags[0].pose.TransformBy(camToTag.best.Inverse()); + frc::Pose3d altPose{}; + if (camToTag.ambiguity != 0) { + altPose = knownTags[0].pose.TransformBy(camToTag.alt.Inverse()); + } + frc::Pose3d o{}; + PNPResult result{}; + result.best = frc::Transform3d{o, bestPose}; + result.alt = frc::Transform3d{o, altPose}; + result.ambiguity = camToTag.ambiguity; + result.bestReprojErr = camToTag.bestReprojErr; + result.altReprojErr = camToTag.altReprojErr; + return result; + } else { + std::vector objectTrls{}; + for (const auto& tag : knownTags) { + auto verts = tagModel.GetFieldVertices(tag.pose); + objectTrls.insert(objectTrls.end(), verts.begin(), verts.end()); + } + PNPResult camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, + objectTrls, points); + if (!camToOrigin.isPresent) { + return PNPResult{}; + } else { + PNPResult result{}; + result.best = camToOrigin.best.Inverse(), + result.alt = camToOrigin.alt.Inverse(), + result.ambiguity = camToOrigin.ambiguity; + result.bestReprojErr = camToOrigin.bestReprojErr; + result.altReprojErr = camToOrigin.altReprojErr; + result.isPresent = true; + return result; + } + } +} + +} // namespace VisionEstimation +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h new file mode 100644 index 0000000000..ff647cd7cf --- /dev/null +++ b/photon-targeting/src/main/native/include/photon/networktables/NTTopicSet.h @@ -0,0 +1,100 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +namespace photon { +class NTTopicSet { + public: + std::shared_ptr subTable; + nt::RawPublisher rawBytesEntry; + + nt::IntegerPublisher pipelineIndexPublisher; + nt::IntegerSubscriber pipelineIndexRequestSub; + + nt::BooleanTopic driverModeEntry; + nt::BooleanPublisher driverModePublisher; + nt::BooleanSubscriber driverModeSubscriber; + + nt::DoublePublisher latencyMillisEntry; + nt::BooleanPublisher hasTargetEntry; + nt::DoublePublisher targetPitchEntry; + nt::DoublePublisher targetYawEntry; + nt::DoublePublisher targetAreaEntry; + nt::DoubleArrayPublisher targetPoseEntry; + nt::DoublePublisher targetSkewEntry; + + nt::DoublePublisher bestTargetPosX; + nt::DoublePublisher bestTargetPosY; + + nt::IntegerTopic heartbeatTopic; + nt::IntegerPublisher heartbeatPublisher; + + nt::DoubleArrayPublisher cameraIntrinsicsPublisher; + nt::DoubleArrayPublisher cameraDistortionPublisher; + + void UpdateEntries() { + nt::PubSubOptions options; + options.periodic = 0.01; + options.sendAll = true; + rawBytesEntry = + subTable->GetRawTopic("rawBytes").Publish("rawBytes", options); + + pipelineIndexPublisher = + subTable->GetIntegerTopic("pipelineIndexState").Publish(); + pipelineIndexRequestSub = + subTable->GetIntegerTopic("pipelineIndexRequest").Subscribe(0); + + driverModePublisher = subTable->GetBooleanTopic("driverMode").Publish(); + driverModeSubscriber = + subTable->GetBooleanTopic("driverModeRequest").Subscribe(0); + + driverModeSubscriber.GetTopic().Publish().SetDefault(false); + + latencyMillisEntry = subTable->GetDoubleTopic("latencyMillis").Publish(); + hasTargetEntry = subTable->GetBooleanTopic("hasTargets").Publish(); + + targetPitchEntry = subTable->GetDoubleTopic("targetPitch").Publish(); + targetAreaEntry = subTable->GetDoubleTopic("targetArea").Publish(); + targetYawEntry = subTable->GetDoubleTopic("targetYaw").Publish(); + targetPoseEntry = subTable->GetDoubleArrayTopic("targetPose").Publish(); + targetSkewEntry = subTable->GetDoubleTopic("targetSkew").Publish(); + + bestTargetPosX = subTable->GetDoubleTopic("targetPixelsX").Publish(); + bestTargetPosY = subTable->GetDoubleTopic("targetPixelsY").Publish(); + + heartbeatTopic = subTable->GetIntegerTopic("heartbeat"); + heartbeatPublisher = heartbeatTopic.Publish(); + + cameraIntrinsicsPublisher = + subTable->GetDoubleArrayTopic("cameraIntrinsics").Publish(); + cameraDistortionPublisher = + subTable->GetDoubleArrayTopic("cameraDistortion").Publish(); + } + + private: +}; +} // namespace photon diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h index e97f6c8d71..49f73ed53b 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h @@ -27,15 +27,15 @@ class PNPResult { public: // This could be wrapped in an std::optional, but chose to do it this way to // mirror Java - bool isPresent; + bool isPresent{false}; - frc::Transform3d best; - double bestReprojErr; + frc::Transform3d best{}; + double bestReprojErr{0}; - frc::Transform3d alt; - double altReprojErr; + frc::Transform3d alt{}; + double altReprojErr{0}; - double ambiguity; + double ambiguity{0}; bool operator==(const PNPResult& other) const; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.gitignore b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore new file mode 100644 index 0000000000..34878ab18c --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/.gitignore @@ -0,0 +1 @@ +vendordeps diff --git a/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000000..a6e62683de --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": true, + "currentLanguage": "cpp", + "projectYear": "2023", + "teamNumber": 5 +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md new file mode 100644 index 0000000000..3d5a824cad --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2021 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/build.gradle b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle new file mode 100644 index 0000000000..8eeef20126 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/build.gradle @@ -0,0 +1,121 @@ +plugins { + id "cpp" + id "google-test-test-suite" + id "edu.wpi.first.GradleRIO" + + id "com.dorongold.task-tree" version "2.1.0" +} + +wpi.maven.useLocal = false +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = '2024.1.1-beta-3-53-g31cd015' +wpi.versions.wpimathVersion = '2024.1.1-beta-3-53-g31cd015' + +repositories { + mavenLocal() + jcenter() +} + +apply from: "${rootDir}/../shared/examples_common.gradle" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamOrDefault(5940) + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcCpp(getArtifactTypeClass('FRCNativeArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcCpp + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Set to true to run simulation in debug mode +wpi.cpp.debugSimulation = false + +// Default enable simgui +wpi.sim.addGui().defaultEnabled = true +// Enable DS but not by default +wpi.sim.addDriverstation() + +model { + components { + frcUserProgram(NativeExecutableSpec) { + // We don't need to build for roborio -- if we do, we need to install + // a roborio toolchain every time we build in CI + // Ideally, we'd be able to set the roborio toolchain as optional, but + // I can't figure out how to set that environment variable from build.gradle + // (see https://github.com/wpilibsuite/native-utils/blob/2917c69fb5094e36d499c465f047dab81c68446c/ToolchainPlugin/src/main/java/edu/wpi/first/toolchain/ToolchainGraphBuildService.java#L71) + // for now, commented out + + // targetPlatform wpi.platforms.roborio + + if (includeDesktopSupport) { + targetPlatform wpi.platforms.desktop + } + + sources.cpp { + source { + srcDir 'src/main/cpp' + include '**/*.cpp', '**/*.cc' + } + exportedHeaders { + srcDir 'src/main/include' + } + } + + // Set deploy task to deploy this component + deployArtifact.component = it + + // Enable run tasks for this component + wpi.cpp.enableExternalTasks(it) + + // Enable simulation for this component + wpi.sim.enable(it) + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. + wpi.cpp.vendor.cpp(it) + wpi.cpp.deps.wpilib(it) + } + } + testSuites { + frcUserProgramTest(GoogleTestTestSuiteSpec) { + testing $.components.frcUserProgram + + sources.cpp { + source { + srcDir 'src/test/cpp' + include '**/*.cpp' + } + } + + // Enable run tasks for this component + wpi.cpp.enableExternalTasks(it) + + wpi.cpp.vendor.cpp(it) + wpi.cpp.deps.wpilib(it) + wpi.cpp.deps.googleTest(it) + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew b/photonlib-cpp-examples/swervedriveposeestsim/gradlew new file mode 100644 index 0000000000..0ef4c1e860 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/gradlew @@ -0,0 +1,241 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# within the Gradle project. +# +# You can find Gradle at https://github.com/gradle/gradle/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit + +APP_NAME="Gradle" +APP_BASE_NAME=${0##*/} + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD=maximum + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + +# Collect all arguments for the java command; +# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of +# shell script including quotes and variable substitutions, so put them in +# double quotes to make sure that they get re-expanded; and +# * put everything else in single quotes, so that it's not re-expanded. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat b/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat new file mode 100644 index 0000000000..f127cfd49d --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/gradlew.bat @@ -0,0 +1,91 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%"=="" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/photonlib-cpp-examples/swervedriveposeestsim/networktables.json b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json new file mode 100644 index 0000000000..fe51488c70 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/networktables.json @@ -0,0 +1 @@ +[] diff --git a/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle new file mode 100644 index 0000000000..44fbca7512 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +rootProject.name = 'aimattarget' + +pluginManagement { + repositories { + mavenLocal() + jcenter() + gradlePluginPortal() + String frcYear = '2024' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json new file mode 100644 index 0000000000..c4b7efd3d8 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-ds.json @@ -0,0 +1,98 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json new file mode 100644 index 0000000000..1605887709 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui-window.json @@ -0,0 +1,64 @@ +{ + "Docking": { + "Data": [] + }, + "MainWindow": { + "GLOBAL": { + "fps": "120", + "height": "910", + "maximized": "0", + "style": "0", + "userScale": "2", + "width": "1550", + "xpos": "-1602", + "ypos": "79" + } + }, + "Window": { + "###/SmartDashboard/VisionSystemSim-main/Sim Field": { + "Collapsed": "0", + "Pos": "199,200", + "Size": "1342,628" + }, + "###FMS": { + "Collapsed": "0", + "Pos": "5,540", + "Size": "283,146" + }, + "###Joysticks": { + "Collapsed": "0", + "Pos": "359,95", + "Size": "796,240" + }, + "###NetworkTables": { + "Collapsed": "0", + "Pos": "865,52", + "Size": "830,620" + }, + "###Other Devices": { + "Collapsed": "0", + "Pos": "1025,20", + "Size": "250,695" + }, + "###System Joysticks": { + "Collapsed": "0", + "Pos": "5,350", + "Size": "192,218" + }, + "###Timing": { + "Collapsed": "0", + "Pos": "5,150", + "Size": "135,127" + }, + "Debug##Default": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "400,400" + }, + "Robot State": { + "Collapsed": "0", + "Pos": "5,20", + "Size": "92,99" + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/simgui.json b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json new file mode 100644 index 0000000000..e1ba9acce8 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/simgui.json @@ -0,0 +1,57 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d" + }, + "windows": { + "/SmartDashboard/VisionSystemSim-main/Sim Field": { + "EstimatedRobotModules": { + "arrows": false, + "image": "swerve_module.png", + "length": 0.30000001192092896, + "width": 0.30000001192092896 + }, + "apriltag": { + "image": "tag-green.png", + "length": 0.6000000238418579, + "width": 0.6000000238418579 + }, + "bottom": 544, + "builtin": "2023 Charged Up", + "cameras": { + "arrowSize": 19, + "arrowWeight": 1.0, + "style": "Hidden" + }, + "height": 8.013679504394531, + "left": 46, + "right": 1088, + "top": 36, + "visibleTargetPoses": { + "image": "tag-blue.png" + }, + "width": 16.541748046875, + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Drive": { + "open": true + }, + "open": true + }, + "photonvision": { + "open": true, + "photonvision": { + "open": true + } + } + } + } +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp new file mode 100644 index 0000000000..4505740955 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/Robot.cpp @@ -0,0 +1,123 @@ +/* + * MIT License + * + * Copyright (c) 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 + +#include +#include + +void Robot::RobotInit() {} + +void Robot::RobotPeriodic() { + drivetrain.Periodic(); + + auto visionEst = vision.GetEstimatedGlobalPose(); + if (visionEst.has_value()) { + auto est = visionEst.value(); + auto estPose = est.estimatedPose.ToPose2d(); + auto estStdDevs = vision.GetEstimationStdDevs(estPose); + drivetrain.AddVisionMeasurement(est.estimatedPose.ToPose2d(), est.timestamp, + estStdDevs); + } + + drivetrain.Log(); +} + +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() { drivetrain.Stop(); } + +void Robot::DisabledExit() {} + +void Robot::AutonomousInit() { + autoTimer.Restart(); + frc::Pose2d pose{1_m, 1_m, frc::Rotation2d{}}; + drivetrain.ResetPose(pose, true); +} + +void Robot::AutonomousPeriodic() { + if (autoTimer.Get() < 10_s) { + drivetrain.Drive(0.5_mps, 0.5_mps, 0.5_rad_per_s, false); + } else { + autoTimer.Stop(); + drivetrain.Stop(); + } +} + +void Robot::AutonomousExit() {} + +void Robot::TeleopInit() {} + +void Robot::TeleopPeriodic() { + double forward = -controller.GetLeftY() * kDriveSpeed; + if (std::abs(forward) < 0.1) { + forward = 0; + } + forward = forwardLimiter.Calculate(forward); + + double strafe = -controller.GetLeftX() * kDriveSpeed; + if (std::abs(strafe) < 0.1) { + strafe = 0; + } + strafe = strafeLimiter.Calculate(strafe); + + double turn = -controller.GetRightX() * kDriveSpeed; + if (std::abs(turn) < 0.1) { + turn = 0; + } + turn = turnLimiter.Calculate(turn); + + drivetrain.Drive(forward * constants::Swerve::kMaxLinearSpeed, + strafe * constants::Swerve::kMaxLinearSpeed, + turn * constants::Swerve::kMaxAngularSpeed, true); +} + +void Robot::TeleopExit() {} + +void Robot::TestInit() {} + +void Robot::TestPeriodic() {} + +void Robot::TestExit() {} + +void Robot::SimulationPeriodic() { + drivetrain.SimulationPeriodic(); + vision.SimPeriodic(drivetrain.GetSimPose()); + + frc::Field2d& debugField = vision.GetSimDebugField(); + debugField.GetObject("EstimatedRobot")->SetPose(drivetrain.GetPose()); + debugField.GetObject("EstimatedRobotModules") + ->SetPoses(drivetrain.GetModulePoses()); + + units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); + units::volt_t loadedBattVolts = + frc::sim::BatterySim::Calculate({totalCurrent}); + frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts); +} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp new file mode 100644 index 0000000000..bf33d7189d --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDrive.cpp @@ -0,0 +1,211 @@ +/* + * MIT License + * + * Copyright (c) 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 "subsystems/SwerveDrive.h" + +#include + +#include +#include + +SwerveDrive::SwerveDrive() + : poseEstimator(kinematics, GetGyroYaw(), GetModulePositions(), + frc::Pose2d{}, {0.1, 0.1, 0.1}, {1.0, 1.0, 1.0}), + gyroSim(gyro), + swerveDriveSim(constants::Swerve::kDriveFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kDriveGearRatio, + constants::Swerve::kWheelDiameter / 2, + constants::Swerve::kSteerFF, frc::DCMotor::Falcon500(1), + constants::Swerve::kSteerGearRatio, kinematics) {} + +void SwerveDrive::Periodic() { + for (auto& currentModule : swerveMods) { + currentModule.Periodic(); + } + + poseEstimator.Update(GetGyroYaw(), GetModulePositions()); +} + +void SwerveDrive::Drive(units::meters_per_second_t vx, + units::meters_per_second_t vy, + units::radians_per_second_t omega, bool openLoop) { + frc::ChassisSpeeds newChassisSpeeds = + frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, GetHeading()); + SetChassisSpeeds(newChassisSpeeds, openLoop, false); +} + +void SwerveDrive::SetChassisSpeeds(const frc::ChassisSpeeds& newChassisSpeeds, + bool openLoop, bool steerInPlace) { + SetModuleStates(kinematics.ToSwerveModuleStates(newChassisSpeeds), openLoop, + steerInPlace); + this->targetChassisSpeeds = newChassisSpeeds; +} + +void SwerveDrive::SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace) { + std::array desaturatedStates = desiredStates; + frc::SwerveDriveKinematics<4>::DesaturateWheelSpeeds( + static_cast*>(&desaturatedStates), + constants::Swerve::kMaxLinearSpeed); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SetDesiredState(desaturatedStates[i], openLoop, steerInPlace); + } +} + +void SwerveDrive::Stop() { Drive(0_mps, 0_mps, 0_rad_per_s, true); } + +void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp) { + poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp); +} + +void SwerveDrive::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp, + const Eigen::Vector3d& stdDevs) { + wpi::array newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)}; + poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs); +} + +void SwerveDrive::ResetPose(const frc::Pose2d& pose, bool resetSimPose) { + if (resetSimPose) { + swerveDriveSim.Reset(pose, false); + for (int i = 0; i < swerveMods.size(); i++) { + swerveMods[i].SimulationUpdate(0_m, 0_mps, 0_A, 0_rad, 0_rad_per_s, 0_A); + } + gyroSim.SetAngle(-pose.Rotation().Degrees()); + gyroSim.SetRate(0_rad_per_s); + } + + poseEstimator.ResetPosition(GetGyroYaw(), GetModulePositions(), pose); +} + +frc::Pose2d SwerveDrive::GetPose() const { + return poseEstimator.GetEstimatedPosition(); +} + +frc::Rotation2d SwerveDrive::GetHeading() const { return GetPose().Rotation(); } + +frc::Rotation2d SwerveDrive::GetGyroYaw() const { return gyro.GetRotation2d(); } + +frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() const { + return kinematics.ToChassisSpeeds(GetModuleStates()); +} + +std::array SwerveDrive::GetModuleStates() const { + std::array moduleStates; + moduleStates[0] = swerveMods[0].GetState(); + moduleStates[1] = swerveMods[1].GetState(); + moduleStates[2] = swerveMods[2].GetState(); + moduleStates[3] = swerveMods[3].GetState(); + return moduleStates; +} + +std::array SwerveDrive::GetModulePositions() + const { + std::array modulePositions; + modulePositions[0] = swerveMods[0].GetPosition(); + modulePositions[1] = swerveMods[1].GetPosition(); + modulePositions[2] = swerveMods[2].GetPosition(); + modulePositions[3] = swerveMods[3].GetPosition(); + return modulePositions; +} + +std::array SwerveDrive::GetModulePoses() const { + std::array modulePoses; + for (int i = 0; i < swerveMods.size(); i++) { + const SwerveModule& module = swerveMods[i]; + modulePoses[i] = GetPose().TransformBy(frc::Transform2d{ + module.GetModuleConstants().centerOffset, module.GetAbsoluteHeading()}); + } + return modulePoses; +} + +void SwerveDrive::Log() { + std::string table = "Drive/"; + frc::Pose2d pose = GetPose(); + frc::SmartDashboard::PutNumber(table + "X", pose.X().to()); + frc::SmartDashboard::PutNumber(table + "Y", pose.Y().to()); + frc::SmartDashboard::PutNumber(table + "Heading", + pose.Rotation().Degrees().to()); + frc::ChassisSpeeds chassisSpeeds = GetChassisSpeeds(); + frc::SmartDashboard::PutNumber(table + "VX", chassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "VY", chassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Omega Degrees", + chassisSpeeds.omega.convert().to()); + frc::SmartDashboard::PutNumber(table + "Target VX", + targetChassisSpeeds.vx.to()); + frc::SmartDashboard::PutNumber(table + "Target VY", + targetChassisSpeeds.vy.to()); + frc::SmartDashboard::PutNumber( + table + "Target Omega Degrees", + targetChassisSpeeds.omega.convert() + .to()); + + for (auto& module : swerveMods) { + module.Log(); + } +} + +void SwerveDrive::SimulationPeriodic() { + std::array driveInputs; + std::array steerInputs; + for (int i = 0; i < swerveMods.size(); i++) { + driveInputs[i] = swerveMods[i].GetDriveVoltage(); + steerInputs[i] = swerveMods[i].GetSteerVoltage(); + } + swerveDriveSim.SetDriveInputs(driveInputs); + swerveDriveSim.SetSteerInputs(steerInputs); + + swerveDriveSim.Update(frc::TimedRobot::kDefaultPeriod); + + auto driveStates = swerveDriveSim.GetDriveStates(); + auto steerStates = swerveDriveSim.GetSteerStates(); + totalCurrentDraw = 0_A; + std::array driveCurrents = + swerveDriveSim.GetDriveCurrentDraw(); + for (const auto& current : driveCurrents) { + totalCurrentDraw += current; + } + std::array steerCurrents = + swerveDriveSim.GetSteerCurrentDraw(); + for (const auto& current : steerCurrents) { + totalCurrentDraw += current; + } + for (int i = 0; i < swerveMods.size(); i++) { + units::meter_t drivePos{driveStates[i](0, 0)}; + units::meters_per_second_t driveRate{driveStates[i](1, 0)}; + units::radian_t steerPos{steerStates[i](0, 0)}; + units::radians_per_second_t steerRate{steerStates[i](1, 0)}; + swerveMods[i].SimulationUpdate(drivePos, driveRate, driveCurrents[i], + steerPos, steerRate, steerCurrents[i]); + } + gyroSim.SetRate(-swerveDriveSim.GetOmega()); + gyroSim.SetAngle(-swerveDriveSim.GetPose().Rotation().Degrees()); +} + +frc::Pose2d SwerveDrive::GetSimPose() const { return swerveDriveSim.GetPose(); } + +units::ampere_t SwerveDrive::GetCurrentDraw() const { return totalCurrentDraw; } diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp new file mode 100644 index 0000000000..31bbe09e61 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveDriveSim.cpp @@ -0,0 +1,285 @@ +/* + * MIT License + * + * Copyright (c) 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 "subsystems/SwerveDriveSim.h" + +#include + +#include +#include + +template +int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +SwerveDriveSim::SwerveDriveSim( + const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : SwerveDriveSim( + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -driveFF.kV.to() / driveFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / driveFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + driveFF.kS, driveMotor, driveGearing, driveWheelRadius, + frc::LinearSystem<2, 1, 2>{ + (Eigen::MatrixXd(2, 2) << 0.0, 1.0, 0.0, + -steerFF.kV.to() / steerFF.kA.to()) + .finished(), + Eigen::Matrix{0.0, 1.0 / steerFF.kA.to()}, + (Eigen::MatrixXd(2, 2) << 1.0, 0.0, 0.0, 1.0).finished(), + Eigen::Matrix{0.0, 0.0}}, + steerFF.kS, steerMotor, steerGearing, kinematics) {} + +SwerveDriveSim::SwerveDriveSim( + const frc::LinearSystem<2, 1, 2>& drivePlant, units::volt_t driveKs, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, units::volt_t steerKs, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics) + : drivePlant(drivePlant), + driveKs(driveKs), + driveMotor(driveMotor), + driveGearing(driveGearing), + driveWheelRadius(driveWheelRadius), + steerPlant(steerPlant), + steerKs(steerKs), + steerMotor(steerMotor), + steerGearing(steerGearing), + kinematics(kinematics) {} + +void SwerveDriveSim::SetDriveInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < driveInputs.size(); i++) { + units::volt_t input = inputs[i]; + driveInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +void SwerveDriveSim::SetSteerInputs( + const std::array& inputs) { + units::volt_t battVoltage = frc::RobotController::GetBatteryVoltage(); + for (int i = 0; i < steerInputs.size(); i++) { + units::volt_t input = inputs[i]; + steerInputs[i] = std::clamp(input, -battVoltage, battVoltage); + } +} + +Eigen::Matrix SwerveDriveSim::CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS) { + auto Ax = discA * x; + double nextStateVel = Ax(1, 0); + double inputToStop = nextStateVel / -discB(1, 0); + double ksSystemEffect = + std::clamp(inputToStop, -kS.to(), kS.to()); + + nextStateVel += discB(1, 0) * ksSystemEffect; + inputToStop = nextStateVel / -discB(1, 0); + double signToStop = sgn(inputToStop); + double inputSign = sgn(input.to()); + double ksInputEffect = 0; + + if (std::abs(ksSystemEffect) < kS.to()) { + double absInput = std::abs(input.to()); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } else if ((input.to() * signToStop) > (inputToStop * signToStop)) { + double absInput = std::abs(input.to() - inputToStop); + ksInputEffect = + -std::clamp(kS.to() * inputSign, -absInput, absInput); + } + + auto sF = Eigen::Matrix{input.to() + ksSystemEffect + + ksInputEffect}; + auto Bu = discB * sF; + auto retVal = Ax + Bu; + return retVal; +} + +void SwerveDriveSim::Update(units::second_t dt) { + Eigen::Matrix driveDiscA; + Eigen::Matrix driveDiscB; + frc::DiscretizeAB<2, 1>(drivePlant.A(), drivePlant.B(), dt, &driveDiscA, + &driveDiscB); + + Eigen::Matrix steerDiscA; + Eigen::Matrix steerDiscB; + frc::DiscretizeAB<2, 1>(steerPlant.A(), steerPlant.B(), dt, &steerDiscA, + &steerDiscB); + + std::array moduleDeltas; + + for (int i = 0; i < numModules; i++) { + double prevDriveStatePos = driveStates[i](0, 0); + driveStates[i] = CalculateX(driveDiscA, driveDiscB, driveStates[i], + driveInputs[i], driveKs); + double currentDriveStatePos = driveStates[i](0, 0); + steerStates[i] = CalculateX(steerDiscA, steerDiscB, steerStates[i], + steerInputs[i], steerKs); + double currentSteerStatePos = steerStates[i](0, 0); + moduleDeltas[i] = frc::SwerveModulePosition{ + units::meter_t{currentDriveStatePos - prevDriveStatePos}, + frc::Rotation2d{units::radian_t{currentSteerStatePos}}}; + } + + frc::Twist2d twist = kinematics.ToTwist2d(moduleDeltas); + pose = pose.Exp(twist); + omega = twist.dtheta / dt; +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, bool preserveMotion) { + this->pose = pose; + if (!preserveMotion) { + for (int i = 0; i < numModules; i++) { + driveStates[i] = Eigen::Matrix{0, 0}; + steerStates[i] = Eigen::Matrix{0, 0}; + } + omega = 0_rad_per_s; + } +} + +void SwerveDriveSim::Reset(const frc::Pose2d& pose, + const std::array, + numModules>& moduleDriveStates, + const std::array, + numModules>& moduleSteerStates) { + this->pose = pose; + driveStates = moduleDriveStates; + steerStates = moduleSteerStates; + omega = kinematics.ToChassisSpeeds(GetModuleStates()).omega; +} + +frc::Pose2d SwerveDriveSim::GetPose() const { return pose; } + +std::array +SwerveDriveSim::GetModulePositions() const { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetNoisyModulePositions(units::meter_t driveStdDev, + units::radian_t steerStdDev) { + std::array positions; + for (int i = 0; i < numModules; i++) { + positions[i] = frc::SwerveModulePosition{ + units::meter_t{driveStates[i](0, 0)} + + randDist(generator) * driveStdDev, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)} + + randDist(generator) * steerStdDev}}; + } + return positions; +} + +std::array +SwerveDriveSim::GetModuleStates() { + std::array states; + for (int i = 0; i < numModules; i++) { + states[i] = frc::SwerveModuleState{ + units::meters_per_second_t{driveStates[i](1, 0)}, + frc::Rotation2d{units::radian_t{steerStates[i](0, 0)}}}; + } + return states; +} + +std::array, numModules> +SwerveDriveSim::GetDriveStates() const { + return driveStates; +} + +std::array, numModules> +SwerveDriveSim::GetSteerStates() const { + return steerStates; +} + +units::radians_per_second_t SwerveDriveSim::GetOmega() const { return omega; } + +units::ampere_t SwerveDriveSim::GetCurrentDraw( + const frc::DCMotor& motor, units::radians_per_second_t velocity, + units::volt_t inputVolts, units::volt_t batteryVolts) const { + units::volt_t effVolts = inputVolts - velocity / motor.Kv; + if (inputVolts >= 0_V) { + effVolts = std::clamp(effVolts, 0_V, inputVolts); + } else { + effVolts = std::clamp(effVolts, inputVolts, 0_V); + } + auto retVal = (inputVolts / batteryVolts) * (effVolts / motor.R); + return retVal; +} + +std::array SwerveDriveSim::GetDriveCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{driveStates[i](1, 0)} * driveGearing / + driveWheelRadius.to(); + currents[i] = GetCurrentDraw(driveMotor, speed, driveInputs[i], + frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +std::array SwerveDriveSim::GetSteerCurrentDraw() + const { + std::array currents; + for (int i = 0; i < numModules; i++) { + units::radians_per_second_t speed = + units::radians_per_second_t{steerStates[i](1, 0) * steerGearing}; + // TODO: If uncommented we get huge current values.. Not sure how to fix + // atm. :( + currents[i] = 20_A; + // currents[i] = GetCurrentDraw(steerMotor, speed, steerInputs[i], + // frc::RobotController::GetBatteryVoltage()); + } + return currents; +} + +units::ampere_t SwerveDriveSim::GetTotalCurrentDraw() const { + units::ampere_t total{0}; + for (const auto& val : GetDriveCurrentDraw()) { + total += val; + } + for (const auto& val : GetSteerCurrentDraw()) { + total += val; + } + return total; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp new file mode 100644 index 0000000000..781c28a338 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/cpp/subsystems/SwerveModule.cpp @@ -0,0 +1,146 @@ +/* + * MIT License + * + * Copyright (c) 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 "subsystems/SwerveModule.h" + +#include + +#include +#include +#include + +SwerveModule::SwerveModule(const constants::Swerve::ModuleConstants& consts) + : moduleConstants(consts), + driveMotor(frc::PWMSparkMax{moduleConstants.driveMotorId}), + driveEncoder(frc::Encoder{moduleConstants.driveEncoderA, + moduleConstants.driveEncoderB}), + steerMotor(frc::PWMSparkMax{moduleConstants.steerMotorId}), + steerEncoder(frc::Encoder{moduleConstants.steerEncoderA, + moduleConstants.steerEncoderB}), + driveEncoderSim(driveEncoder), + steerEncoderSim(steerEncoder) { + driveEncoder.SetDistancePerPulse( + constants::Swerve::kDriveDistPerPulse.to()); + steerEncoder.SetDistancePerPulse( + constants::Swerve::kSteerRadPerPulse.to()); + steerPIDController.EnableContinuousInput(-std::numbers::pi, std::numbers::pi); +} + +void SwerveModule::Periodic() { + units::volt_t steerPID = units::volt_t{ + steerPIDController.Calculate(GetAbsoluteHeading().Radians().to(), + desiredState.angle.Radians().to())}; + steerMotor.SetVoltage(steerPID); + + units::volt_t driveFF = + constants::Swerve::kDriveFF.Calculate(desiredState.speed); + units::volt_t drivePID{0}; + if (!openLoop) { + drivePID = units::volt_t{drivePIDController.Calculate( + driveEncoder.GetRate(), desiredState.speed.to())}; + } + driveMotor.SetVoltage(driveFF + drivePID); +} + +void SwerveModule::SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace) { + frc::Rotation2d currentRotation = GetAbsoluteHeading(); + frc::SwerveModuleState optimizedState = + frc::SwerveModuleState::Optimize(newState, currentRotation); + desiredState = optimizedState; +} + +frc::Rotation2d SwerveModule::GetAbsoluteHeading() const { + return frc::Rotation2d{units::radian_t{steerEncoder.GetDistance()}}; +} + +frc::SwerveModuleState SwerveModule::GetState() const { + return frc::SwerveModuleState{driveEncoder.GetRate() * 1_mps, + GetAbsoluteHeading()}; +} + +frc::SwerveModulePosition SwerveModule::GetPosition() const { + return frc::SwerveModulePosition{driveEncoder.GetDistance() * 1_m, + GetAbsoluteHeading()}; +} + +units::volt_t SwerveModule::GetDriveVoltage() const { + return driveMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::volt_t SwerveModule::GetSteerVoltage() const { + return steerMotor.Get() * frc::RobotController::GetBatteryVoltage(); +} + +units::ampere_t SwerveModule::GetDriveCurrentSim() const { + return driveCurrentSim; +} + +units::ampere_t SwerveModule::GetSteerCurrentSim() const { + return steerCurrentSim; +} + +constants::Swerve::ModuleConstants SwerveModule::GetModuleConstants() const { + return moduleConstants; +} + +void SwerveModule::Log() { + frc::SwerveModuleState state = GetState(); + + std::string table = + "Module " + std::to_string(moduleConstants.moduleNum) + "/"; + frc::SmartDashboard::PutNumber(table + "Steer Degrees", + frc::AngleModulus(state.angle.Radians()) + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Steer Target Degrees", + units::radian_t{steerPIDController.GetSetpoint()} + .convert() + .to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Feet", + state.speed.convert().to()); + frc::SmartDashboard::PutNumber( + table + "Drive Velocity Target Feet", + desiredState.speed.convert().to()); + frc::SmartDashboard::PutNumber(table + "Drive Current", + driveCurrentSim.to()); + frc::SmartDashboard::PutNumber(table + "Steer Current", + steerCurrentSim.to()); +} + +void SwerveModule::SimulationUpdate( + units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent) { + driveEncoderSim.SetDistance(driveEncoderDist.to()); + driveEncoderSim.SetRate(driveEncoderRate.to()); + driveCurrentSim = driveCurrent; + steerEncoderSim.SetDistance(steerEncoderDist.to()); + steerEncoderSim.SetRate(steerEncoderRate.to()); + steerCurrentSim = steerCurrent; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt b/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt new file mode 100644 index 0000000000..15bc5c1ebe --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/deploy/example.txt @@ -0,0 +1,4 @@ +Files placed in this directory will be deployed to the RoboRIO into the + 'deploy' directory in the home folder. Use the 'frc::filesystem::GetDeployDirectory' + function from the 'frc/Filesystem.h' header to get a proper path relative to the deploy + directory. diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h new file mode 100644 index 0000000000..70be37d406 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Constants.h @@ -0,0 +1,118 @@ +/* + * MIT License + * + * Copyright (c) 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 + +#include +#include +#include +#include +#include +#include + +namespace constants { +namespace Vision { +static constexpr std::string_view kCameraName{"YOUR CAMERA NAME"}; +static const frc::Transform3d kRobotToCam{ + frc::Translation3d{0.5_m, 0.0_m, 0.5_m}, + frc::Rotation3d{0_rad, 0_rad, 0_rad}}; +static const frc::AprilTagFieldLayout kTagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp)}; + +static const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; +static const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; +} // namespace Vision +namespace Swerve { + +static constexpr units::meter_t kTrackWidth{18.5_in}; +static constexpr units::meter_t kTrackLength{18.5_in}; +static constexpr units::meter_t kRobotWidth{25_in + 3.25_in * 2}; +static constexpr units::meter_t kRobotLength{25_in + 3.25_in * 2}; +static constexpr units::meters_per_second_t kMaxLinearSpeed{15.5_fps}; +static constexpr units::radians_per_second_t kMaxAngularSpeed{720_deg_per_s}; +static constexpr units::meter_t kWheelDiameter{4_in}; +static constexpr units::meter_t kWheelCircumference{kWheelDiameter * + std::numbers::pi}; + +static constexpr double kDriveGearRatio = 6.75; +static constexpr double kSteerGearRatio = 12.8; + +static constexpr units::meter_t kDriveDistPerPulse = + kWheelCircumference / 1024.0 / kDriveGearRatio; +static constexpr units::radian_t kSteerRadPerPulse = + units::radian_t{2 * std::numbers::pi} / 1024.0; + +static constexpr double kDriveKP = 1.0; +static constexpr double kDriveKI = 0.0; +static constexpr double kDriveKD = 0.0; + +static constexpr double kSteerKP = 20.0; +static constexpr double kSteerKI = 0.0; +static constexpr double kSteerKD = 0.25; + +static const frc::SimpleMotorFeedforward kDriveFF{ + 0.25_V, 2.5_V / 1_mps, 0.3_V / 1_mps_sq}; + +static const frc::SimpleMotorFeedforward kSteerFF{ + 0.5_V, 0.25_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; + +struct ModuleConstants { + public: + const int moduleNum; + const int driveMotorId; + const int driveEncoderA; + const int driveEncoderB; + const int steerMotorId; + const int steerEncoderA; + const int steerEncoderB; + const double angleOffset; + const frc::Translation2d centerOffset; + + ModuleConstants(int moduleNum, int driveMotorId, int driveEncoderA, + int driveEncoderB, int steerMotorId, int steerEncoderA, + int steerEncoderB, double angleOffset, units::meter_t xOffset, + units::meter_t yOffset) + : moduleNum(moduleNum), + driveMotorId(driveMotorId), + driveEncoderA(driveEncoderA), + driveEncoderB(driveEncoderB), + steerMotorId(steerMotorId), + steerEncoderA(steerEncoderA), + steerEncoderB(steerEncoderB), + angleOffset(angleOffset), + centerOffset(frc::Translation2d{xOffset, yOffset}) {} +}; + +static const ModuleConstants FL_CONSTANTS{ + 1, 0, 0, 1, 1, 2, 3, 0, kTrackLength / 2, kTrackWidth / 2}; +static const ModuleConstants FR_CONSTANTS{ + 2, 2, 4, 5, 3, 6, 7, 0, kTrackLength / 2, -kTrackWidth / 2}; +static const ModuleConstants BL_CONSTANTS{ + 3, 4, 8, 9, 5, 10, 11, 0, -kTrackLength / 2, kTrackWidth / 2}; +static const ModuleConstants BR_CONSTANTS{ + 4, 6, 12, 13, 7, 14, 15, 0, -kTrackLength / 2, -kTrackWidth / 2}; +} // namespace Swerve +} // namespace constants diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h new file mode 100644 index 0000000000..7a3697494a --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Robot.h @@ -0,0 +1,62 @@ +/* + * MIT License + * + * Copyright (c) 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 +#include +#include +#include + +#include "Vision.h" +#include "subsystems/SwerveDrive.h" + +class Robot : public frc::TimedRobot { + public: + void RobotInit() override; + void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void DisabledExit() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void AutonomousExit() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TeleopExit() override; + void TestInit() override; + void TestPeriodic() override; + void TestExit() override; + void SimulationPeriodic() override; + + private: + SwerveDrive drivetrain{}; + Vision vision{}; + frc::XboxController controller{0}; + frc::SlewRateLimiter forwardLimiter{1.0 / 0.6_s}; + frc::SlewRateLimiter strafeLimiter{1.0 / 0.6_s}; + frc::SlewRateLimiter turnLimiter{1.0 / 0.33_s}; + frc::Timer autoTimer{}; + double kDriveSpeed{0.6}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h new file mode 100644 index 0000000000..619b34ade7 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h @@ -0,0 +1,152 @@ +/* + * MIT License + * + * Copyright (c) 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 +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "Constants.h" + +class Vision { + public: + Vision() { + photonEstimator.SetMultiTagFallbackStrategy( + photon::PoseStrategy::LOWEST_AMBIGUITY); + + if (frc::RobotBase::IsSimulation()) { + visionSim = std::make_unique("main"); + + visionSim->AddAprilTags(constants::Vision::kTagLayout); + + cameraProp = std::make_unique(); + + cameraProp->SetCalibration(960, 720, frc::Rotation2d{90_deg}); + cameraProp->SetCalibError(.35, .10); + cameraProp->SetFPS(15_Hz); + cameraProp->SetAvgLatency(50_ms); + cameraProp->SetLatencyStdDev(15_ms); + + cameraSim = std::make_shared(camera.get(), + *cameraProp.get()); + + visionSim->AddCamera(cameraSim.get(), robotToCam); + cameraSim->EnableDrawWireframe(true); + } + } + + photon::PhotonPipelineResult GetLatestResult() { + return camera->GetLatestResult(); + } + + std::optional GetEstimatedGlobalPose() { + auto visionEst = photonEstimator.Update(); + units::second_t latestTimestamp = camera->GetLatestResult().GetTimestamp(); + bool newResult = + units::math::abs(latestTimestamp - lastEstTimestamp) > 1e-5_s; + if (frc::RobotBase::IsSimulation()) { + if (visionEst.has_value()) { + GetSimDebugField() + .GetObject("VisionEstimation") + ->SetPose(visionEst.value().estimatedPose.ToPose2d()); + } else { + if (newResult) { + GetSimDebugField().GetObject("VisionEstimation")->SetPoses({}); + } + } + } + if (newResult) { + lastEstTimestamp = latestTimestamp; + } + return visionEst; + } + + Eigen::Matrix GetEstimationStdDevs(frc::Pose2d estimatedPose) { + Eigen::Matrix estStdDevs = + constants::Vision::kSingleTagStdDevs; + auto targets = GetLatestResult().GetTargets(); + int numTags = 0; + units::meter_t avgDist = 0_m; + for (const auto& tgt : targets) { + auto tagPose = + photonEstimator.GetFieldLayout().GetTagPose(tgt.GetFiducialId()); + if (tagPose.has_value()) { + numTags++; + avgDist += tagPose.value().ToPose2d().Translation().Distance( + estimatedPose.Translation()); + } + } + if (numTags == 0) { + return estStdDevs; + } + avgDist /= numTags; + if (numTags > 1) { + estStdDevs = constants::Vision::kMultiTagStdDevs; + } + if (numTags == 1 && avgDist > 4_m) { + estStdDevs = (Eigen::MatrixXd(3, 1) << std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()) + .finished(); + } else { + estStdDevs = estStdDevs * (1 + (avgDist.value() * avgDist.value() / 30)); + } + return estStdDevs; + } + + void SimPeriodic(frc::Pose2d robotSimPose) { + visionSim->Update(robotSimPose); + } + + void ResetSimPose(frc::Pose2d pose) { + if (frc::RobotBase::IsSimulation()) { + visionSim->ResetRobotPose(pose); + } + } + + frc::Field2d& GetSimDebugField() { return visionSim->GetDebugField(); } + + private: + frc::Transform3d robotToCam{frc::Translation3d{0.5_m, 0.5_m, 0.5_m}, + frc::Rotation3d{}}; + photon::PhotonPoseEstimator photonEstimator{ + LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp), + photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR, + photon::PhotonCamera{"photonvision"}, robotToCam}; + std::shared_ptr camera{photonEstimator.GetCamera()}; + std::unique_ptr visionSim; + std::unique_ptr cameraProp; + std::shared_ptr cameraSim; + units::second_t lastEstTimestamp{0_s}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h new file mode 100644 index 0000000000..1e3d26b32f --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDrive.h @@ -0,0 +1,85 @@ +/* + * MIT License + * + * Copyright (c) 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 +#include +#include +#include +#include + +#include "SwerveDriveSim.h" +#include "SwerveModule.h" + +class SwerveDrive { + public: + SwerveDrive(); + void Periodic(); + void Drive(units::meters_per_second_t vx, units::meters_per_second_t vy, + units::radians_per_second_t omega, bool openLoop); + void SetChassisSpeeds(const frc::ChassisSpeeds& targetChassisSpeeds, + bool openLoop, bool steerInPlace); + void SetModuleStates( + const std::array& desiredStates, bool openLoop, + bool steerInPlace); + void Stop(); + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp); + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp, + const Eigen::Vector3d& stdDevs); + void ResetPose(const frc::Pose2d& pose, bool resetSimPose); + frc::Pose2d GetPose() const; + frc::Rotation2d GetHeading() const; + frc::Rotation2d GetGyroYaw() const; + frc::ChassisSpeeds GetChassisSpeeds() const; + std::array GetModuleStates() const; + std::array GetModulePositions() const; + std::array GetModulePoses() const; + void Log(); + void SimulationPeriodic(); + frc::Pose2d GetSimPose() const; + units::ampere_t GetCurrentDraw() const; + + private: + std::array swerveMods{ + SwerveModule{constants::Swerve::FL_CONSTANTS}, + SwerveModule{constants::Swerve::FR_CONSTANTS}, + SwerveModule{constants::Swerve::BL_CONSTANTS}, + SwerveModule{constants::Swerve::BR_CONSTANTS}}; + frc::SwerveDriveKinematics<4> kinematics{ + swerveMods[0].GetModuleConstants().centerOffset, + swerveMods[1].GetModuleConstants().centerOffset, + swerveMods[2].GetModuleConstants().centerOffset, + swerveMods[3].GetModuleConstants().centerOffset, + }; + frc::ADXRS450_Gyro gyro{frc::SPI::Port::kOnboardCS0}; + frc::SwerveDrivePoseEstimator<4> poseEstimator; + frc::ChassisSpeeds targetChassisSpeeds{}; + + frc::sim::ADXRS450_GyroSim gyroSim; + SwerveDriveSim swerveDriveSim; + units::ampere_t totalCurrentDraw{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h new file mode 100644 index 0000000000..c1cee3e6f1 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveDriveSim.h @@ -0,0 +1,102 @@ +/* + * MIT License + * + * Copyright (c) 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 + +#include +#include +#include +#include +#include + +static constexpr int numModules{4}; + +class SwerveDriveSim { + public: + SwerveDriveSim(const frc::SimpleMotorFeedforward& driveFF, + const frc::DCMotor& driveMotor, double driveGearing, + units::meter_t driveWheelRadius, + const frc::SimpleMotorFeedforward& steerFF, + const frc::DCMotor& steerMotor, double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + SwerveDriveSim(const frc::LinearSystem<2, 1, 2>& drivePlant, + units::volt_t driveKs, const frc::DCMotor& driveMotor, + double driveGearing, units::meter_t driveWheelRadius, + const frc::LinearSystem<2, 1, 2>& steerPlant, + units::volt_t steerKs, const frc::DCMotor& steerMotor, + double steerGearing, + const frc::SwerveDriveKinematics& kinematics); + void SetDriveInputs(const std::array& inputs); + void SetSteerInputs(const std::array& inputs); + static Eigen::Matrix CalculateX( + const Eigen::Matrix& discA, + const Eigen::Matrix& discB, + const Eigen::Matrix& x, units::volt_t input, + units::volt_t kS); + void Update(units::second_t dt); + void Reset(const frc::Pose2d& pose, bool preserveMotion); + void Reset(const frc::Pose2d& pose, + const std::array, numModules>& + moduleDriveStates, + const std::array, numModules>& + moduleSteerStates); + frc::Pose2d GetPose() const; + std::array GetModulePositions() const; + std::array GetNoisyModulePositions( + units::meter_t driveStdDev, units::radian_t steerStdDev); + std::array GetModuleStates(); + std::array, numModules> GetDriveStates() const; + std::array, numModules> GetSteerStates() const; + units::radians_per_second_t GetOmega() const; + units::ampere_t GetCurrentDraw(const frc::DCMotor& motor, + units::radians_per_second_t velocity, + units::volt_t inputVolts, + units::volt_t batteryVolts) const; + std::array GetDriveCurrentDraw() const; + std::array GetSteerCurrentDraw() const; + units::ampere_t GetTotalCurrentDraw() const; + + private: + std::random_device rd{}; + std::mt19937 generator{rd()}; + std::normal_distribution randDist{0.0, 1.0}; + const frc::LinearSystem<2, 1, 2> drivePlant; + const units::volt_t driveKs; + const frc::DCMotor driveMotor; + const double driveGearing; + const units::meter_t driveWheelRadius; + const frc::LinearSystem<2, 1, 2> steerPlant; + const units::volt_t steerKs; + const frc::DCMotor steerMotor; + const double steerGearing; + const frc::SwerveDriveKinematics kinematics; + std::array driveInputs{}; + std::array, numModules> driveStates{}; + std::array steerInputs{}; + std::array, numModules> steerStates{}; + frc::Pose2d pose{frc::Pose2d{}}; + units::radians_per_second_t omega{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h new file mode 100644 index 0000000000..444e4bdf44 --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/main/include/subsystems/SwerveModule.h @@ -0,0 +1,81 @@ +/* + * MIT License + * + * Copyright (c) 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 +#include +#include +#include +#include +#include +#include + +#include "Constants.h" + +class SwerveModule { + public: + explicit SwerveModule(const constants::Swerve::ModuleConstants& consts); + void Periodic(); + void SetDesiredState(const frc::SwerveModuleState& newState, + bool shouldBeOpenLoop, bool steerInPlace); + frc::Rotation2d GetAbsoluteHeading() const; + frc::SwerveModuleState GetState() const; + frc::SwerveModulePosition GetPosition() const; + units::volt_t GetDriveVoltage() const; + units::volt_t GetSteerVoltage() const; + units::ampere_t GetDriveCurrentSim() const; + units::ampere_t GetSteerCurrentSim() const; + constants::Swerve::ModuleConstants GetModuleConstants() const; + void Log(); + void SimulationUpdate(units::meter_t driveEncoderDist, + units::meters_per_second_t driveEncoderRate, + units::ampere_t driveCurrent, + units::radian_t steerEncoderDist, + units::radians_per_second_t steerEncoderRate, + units::ampere_t steerCurrent); + + private: + const constants::Swerve::ModuleConstants moduleConstants; + + frc::PWMSparkMax driveMotor; + frc::Encoder driveEncoder; + frc::PWMSparkMax steerMotor; + frc::Encoder steerEncoder; + + frc::SwerveModuleState desiredState{}; + bool openLoop{false}; + + frc::PIDController drivePIDController{constants::Swerve::kDriveKP, + constants::Swerve::kDriveKI, + constants::Swerve::kDriveKD}; + frc::PIDController steerPIDController{constants::Swerve::kSteerKP, + constants::Swerve::kSteerKI, + constants::Swerve::kSteerKD}; + + frc::sim::EncoderSim driveEncoderSim; + units::ampere_t driveCurrentSim{0}; + frc::sim::EncoderSim steerEncoderSim; + units::ampere_t steerCurrentSim{0}; +}; diff --git a/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp new file mode 100644 index 0000000000..031d1ce96b --- /dev/null +++ b/photonlib-cpp-examples/swervedriveposeestsim/src/test/cpp/main.cpp @@ -0,0 +1,34 @@ +/* + * MIT License + * + * Copyright (c) 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 + +#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; +} diff --git a/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png b/photonlib-cpp-examples/swervedriveposeestsim/swerve_module.png new file mode 100644 index 0000000000000000000000000000000000000000..25990c8399b37db4290551cf5b746d1b63108f8a GIT binary patch literal 2954 zcmd6p{Xf&|AIGoHHn!aB)S}Y1bA&p%>4uTmMoM);s1TuY8W_zn6QXs42#)=`~;1rCWD8q*!PxsLKU0@H?Y5@EYiwelObj0E2NW4!F4B+0)|)KJ|06ZmNpI-z#n;xRvR zqWzDSmskxwJ)p-%`D}!74j)?#?IN6*=WGAo(oW;-g>hgGD|^P3 zs`SBIPOW2J6=xTWgHR%1XqvVQoWI+xq`d!<_vG95z4yK0&!Obq&hwj`1wXdjKP3B z4_0z8tohEnou*{?3ncZ~n5U@%sY^CBw$U_0!A_CtQvC4xkm|Y(z#*~RDH!kM3Jj9U zUC*G_X#$U=QRhDU>_lL`;BK>RkoXZ|EV?_d!Dah_z@1xz1e09A#ig*m~F$k)s4&IKFaLNj?W7@?27S zY_L~=lc-1cwub#zG!~tdR_yUS-b(GM`C>yJx+WGpKkgl}!U`)<(u6VbUdpGU5SI}d zEN6Dt!OROMI@H;slBFp}!h@#q(>DgsKt5c00NY_;3pH# ztRoEcetiBj9$uqJ{Yn?-qLM$$BD7wNq&J6Hf*5hQXV1&R-jtdsaB-Q%P3e9Ux7#ER z*~yfoVH|dkbu^dhQ0p;9_;zcb_iD`p>UjlcF(&Rv*@;s}8`bm6;3mRW^YS5N#t@Xh z;+xZ+hEwMQeVl2=nGOd&{cEJM0zSd{2 z!fr)@Kkd^nqndSQCpLsfBRTW*rOXcso*y7LQOmB<(Ka{K>Gc3jsV>}!rftXq9u&8s zx?9aJbf_;dJB)+pZWwV&)tV>NC!Ulmo3r56iz&7H4No^{JUxeqJ;y@^O$7hws+o0& zIRT<7q~q*A5uV1u`~f4*fPB-HhB9UIU%`bfdoa;iMB3Z?UDv{(5D6}EYzS3206h`X z=j}3!`1RCldqD-^`?o?PPMLh)l6i%)c^_mh#t|yW&_PT2R+I_oLq9`OuxHzYV`oteF zLT(ZguEArdUS&XAw!kSj(KIm((J8Tw6J$cHy_){XF>*ocG*TdMwteSjIClUBqU%)wpU`vr!O~w#n91&0s1$A@ zwe?%JqUr$GkWdMIuieZ8CSj9G4=$LpFqvLp?7dhQR}r6?Ci3Htaj$sX7sE7q&d(@< z$q_Wzt_K)rVEe?(HPfNN>7^xKjdn1&cX4~j5Z?+$`^P}y4jk!}gjf^`=@ zN$6vchGDX|3hj|yrf`O$MFz}bRqmE#SZMF!AgD%COJ#5q1b&7SOCFC&!}WnD(xyeR zm(n<+)~p3m`;PYuVGKVN>a*!!X=@`eQ6wglYo)#?kv?*FZ{jA@xl56V6Vyq41ArEs zfsOBzR!=q!@vA>SeXrM!pDGxw<8H<7zPQXZ6}2^iPk7ikjDbKpNUQOH=7;@0stLsP0al&%!f2eK#O@>I~TnpB|zuxrH2~b-Ua-w5waCdfgm6>|dTtNRT zOX`s9p7dWCnF<(&*2LWll}fvhrE1tk zPq1TlAYK}pM+Vj#z&3OK5T*YGr18W(`93AA2gp!s_>&wp85?d9R|uks;O?irjR;SJH1_@5>TTBIR$WMA8qT@o@oWa)RY$qJlj*Cgy7jqC?^xAOEJh_I?y@k6 zU>e^$K-hC0AaFovcueFg04N)fRO8B{+rh9XC_m;K`*p~X=1?B;YEL?bC|v70;{QQ3 z_jDbs)Zhyc-oo3lpYw8{` z!8E)g>rUNR<|@MdEvKB@Cq%eAa$0-aO@ymob!8qRU*0LI*H`3vLUjSMgYdPgHeUPD z+S$Qa^$HIW?v$Ku=rk=2<%LLa7{A^D$^h78oyFtO*(t7NkzQwB8}O9tHCm3e)e`Cy9P- zbPV1maqh&HEO>oO z(7|)lxPIu%us(P^PwKp?h|yO4fzRg=g4o9wPa&EiGGgg&VtveZC>%!0R!^mDfx;C@ z*+!`pZ73Xyr|rp~eV`Aj%6`o+?oQe#d*e0Gf-Gh#a6k{M(pEBFp%3)TR8j=0V)BZA zk{5OwtuQPpYzn1Vdv9BgLs9>pP~%4YYkUOks8mmht=g*bSjpO8Sz!(J_YkB_h6=Cn o+ow(@pSLe?B8BF7pj(6~s>?;ov@y`RA$JBR&MHa@if05Kai!2kdN literal 0 HcmV?d00001 diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-blue.png new file mode 100644 index 0000000000000000000000000000000000000000..04b9e4f7b7241c94a87249c24fb09c2577b2cf44 GIT binary patch literal 4711 zcmeHL`BPI@6uy8KL`Weqf;O_r;;<=N1zgHf6rvI=C?--RfQSo_M3w-8rcOr_vEWE? z83-NQ0%HxENFvITK^Bq4b{w#p7?~hg1VIc1WRaemK>H80{V6|W&U^2E_nhzC`_6aX zjbIPW-`H@aAwtNQ8sHaxE-pV>SB!(F;Vy4iC{BwiC+C(?cnn+J4 z=~|}*Iqum`u{c(=U9f^de>ycFnNIy>ugiMoaC09SnGruQoMENu zA`=@WLTCLb$omps4;c`Fr;iN*Sp_uephW@?#IrKhM<%vfkah{QZ-KTDv|8~0%c0wZ zmm)UYQXJGh7@ww_-{02TJC;?Qdgiqx<3`cZ>9Kcz$}G=V#>(qYO{c0RC*Ms?d+CeL zQ7I^KhR6<+J+L0(_1!!-dyXoaC<%~x%bK5$jZ*Cs1|x|2$k?*z^;f5I zst3L;gceZl9V%*bP$K4y@uHKF@!vpuaL&ILW)HbLRhT6_SrQE`BD8sL{0bNmT_T_O zfv?9SlM)WXV|KVJ?j2s&A0_cUP>)%t$QYh_JiL&k05JReoARyDR(d2N`fGo{J+|-f z{}QZ&EE9sCAaRg%T}ZXe6l^BFLU9h7Qm1eG!K>IFU)x_qa`geUS%f2(0dDV_J9W2V zY?Gi`h39q+HOUhj)&SA3g*WAGIFh}H9=-$SxJNf_#Xb9I0-u{j*^pxO?YB-)<$| zOld<$22kL+*%0W0>gIgDUNhf=8Qq^8-YJCc9dk{V;hE#Xyhx>#M<&pL0rMhX@9r^R zi!UYzmQVD_1$rpuMwtBS27zAlwMHFEVhQ<{+7It1*V@P`+G2Ejb^2n>|zmaC#*!d)S zjuT4gB_TTK$ndWhx@-yNQAB+<>Hb2*yxYL$KmAI%HqB*uy$l@Dp{e>tIcfUR+}PeY zO<6gqHv#Rhv}A3uA(*dLqS8nbn>#%!?xj(X)F`f)6|`7>g-Ti1Y)j~|A(YTQg*{WJ z|M)w}D$VSegxI*LWq7ILUvVcCS#Fv|H`(c%YPS-dzBl$}%tRB@T`(VA`sm<^d4Du^$0=5e z2F%N??ppv$cecAMM0Z+uzG@&u&7`9{f9|)4_StOrGK;#?D&hSft7$LC)o)Q=*#!5r z%Ps<|y>M6Zpj2@DzXlg8oV=sG?shBf2ud!APp=-`5jBUh;o zi$u*tWgM=&%>Cg)3-~1EH63`Q8FKEt+^ZS7J(i-@1)HJ3mqj_zQ10H(S3<(L9*>o1 zeCKa$k!3-}t{*RAJ!wtDVaL1cHgEI8Rc)szum{%+BC7=tgksDRJe|2*L<_>y@<^;= zEF&FsVU}7>7{WRzDO9?nMwpGQZ;cR3k?V2R)FvUDfF;P_){{2SOesSk88iX=j4v5f z%hy|=NpcN@$PYv`7S7Ir#FBul@^bi+R1B@enULbCFyzk=cIJTR0$WA@F1VkINZNb` z+)uW?xr6ml=>n=MeI$^mMYzg6BagNQqLi->EpD>{rM7F7-G)r)I-}yPFl~7(CRn}< zUp{=3_yN@G^BK==*+kYL$9>!UyWJwMsaMJB_CSA0Pn(>2n?2CGRy-k58 zhlpkmzZW0+cmV4{$5pCa`C=}mG9&%>nvcuk2m3M!ph+q_C6g?Ig4G@(v`e5pN@@?v k+Jo}Hd{91lsCH^;si4Yp)^TGd{5gcE+i8BKzRa_K0jU-?_5c6? literal 0 HcmV?d00001 diff --git a/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png b/photonlib-cpp-examples/swervedriveposeestsim/tag-green.png new file mode 100644 index 0000000000000000000000000000000000000000..63029fcf205d2a1b52eb450aa1e26fbdfd310586 GIT binary patch literal 4698 zcmeHLYg3a~5I*5jKx+ge)e0&gii(5a6%bGmxikcdU<|@&07sk_2r+;_t4$FlTolv+ zLJ^(TkqjkV6#{Ze#3)q>+DXCEM4%CpMhuogNfFWR2{8QwZ9nBhX5Vx6d3T@Pb9T>p zlk<77|Dpve79fNc1qS%;N63JQkm1LsAHj@=-}q$^1lE3kA5_z0t%iY7oHxZAq56V_ z@1l)iY?c%d!9s}GiGKvk^`Eg3vUnBf>m8nUSR?biL^`;P9jzprnN`{yT`j)3bji&n zq+iLl$5s<7?d+31ioFEuqqoH*`|qAf-5pZ&g1Y~Lf+`R3G$R_q-!ONX&=hZlhZum`}dj;CRSR;k<)Syudg#^ zMg|S3ckPMD{j_OwwMHcwjV7fD%et%czNsd}1}T(}>=Q3GecN=tQT_aSkg^X$R}6$a zzCO8*B{d_jRUrDed|F9}m>}}eP)5XZ12`6Ft}cQgI47tvo0S!$hj29T^DE_BIH0k0 z?DK*%!*QMRHcSt7MBy`|V{6f%C;HG8Z{OLLcVn3zc7j zv3Vqx$^m3me_o3^D-?(}yN9)?NZ^S=tM|g55+CNKp&jj7fBlSOuC*eV2GAzy$5qjT+iMYgD&@99f?WkOReAidxIABHq-y7ttAA-E!fn zbglYLiJC;NJAXbQEoimdb%}Q5xpMsbzsfC`qU9DQn*v;vzShouV#31k?~b;TOpx=2 zXC+&64DWMcYNyt+ieq@6<4RwvT*0yab6tK4qIGzJ?ZZp&Ur>Sb4YexS98@^ao|;zMBbpWans#s)jyrR{7tah0eJ zE|%^>HAk%&WnI}JwZb-YU93slr^lNh$vBZ4cMwhrRx@^u!R@L0N$T54nkbQ0vS=A4lJO@&t4C_q4k_yyE z`*B4!o$FQG;z~XArOviZoCL28V&}K3WyuYPLB(xv)FlE6JV(%ltF%}wV`4=S#&qJ> zxyDseDNq|yie+ltSGH1l&#_FQ@*eB@iUmW3%~qK+RL~&VdB2r)P#mtDff$@RU%b}! zBC0~;j^Ej#&}4x8Gal2-BIDNLTqV^-_dqPu;4WydNLYm-_Bu!;Zit~&-UQsb+5S0# zDLfA02dkh3qrr&sRA9EJ%@|&!xsCS$h<*cS&}=O*r`~^&{3)2?XQf4c0)5BPW}q0h zjwn+@8LS```@o|?M7h&7jv@QpZk{Ssk>CkvqxduNR>!4X*EoVVko70yzYA?02Kq)5 zGYqaMC#tg5D_Ij`WrLGvTR*%#Hk~F+pV=Vqd$epN^cE#qd}kw#XQ2OB(0?z%_SAne l>QkWK1^;Rx47a6?FD6iOngjQS4#WRJC~#k}Z;cN<^KVFxHsJsO literal 0 HcmV?d00001 diff --git a/shared/javacpp/setupBuild.gradle b/shared/javacpp/setupBuild.gradle index 66106c6057..dd8a5bec32 100644 --- a/shared/javacpp/setupBuild.gradle +++ b/shared/javacpp/setupBuild.gradle @@ -88,6 +88,8 @@ model { } } + nativeUtils.useRequiredLibrary(it, "cscore_shared") + nativeUtils.useRequiredLibrary(it, "cameraserver_shared") nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared") nativeUtils.useRequiredLibrary(it, "googletest_static") nativeUtils.useRequiredLibrary(it, "apriltag_shared") From 6b8882fe53c66ba77ed29cc1a9a81d61a45c0353 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 16 Dec 2023 15:26:00 -0500 Subject: [PATCH 3/3] Report cpp usage (#1045) --- .../src/main/native/cpp/photon/PhotonCamera.cpp | 7 ++++++- .../main/native/cpp/photon/PhotonPoseEstimator.cpp | 14 ++++++++++++-- .../src/main/native/include/photon/PhotonCamera.h | 1 + .../native/include/photon/PhotonPoseEstimator.h | 2 ++ 4 files changed, 21 insertions(+), 3 deletions(-) diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index ff26f74fb8..02d9b46527 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -24,6 +24,8 @@ #include "photon/PhotonCamera.h" +#include + #include #include #include @@ -69,7 +71,10 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, rootTable->GetBooleanTopic("driverModeRequest").Publish()), m_topicNameSubscriber(instance, PHOTON_PREFIX, {.topicsOnly = true}), path(rootTable->GetPath()), - m_cameraName(cameraName) {} + m_cameraName(cameraName) { + HAL_Report(HALUsageReporting::kResourceType_PhotonCamera, InstanceCount); + InstanceCount++; +} PhotonCamera::PhotonCamera(const std::string_view cameraName) : PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {} diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index f2ff30f7ea..30bc6de2df 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -24,6 +24,8 @@ #include "photon/PhotonPoseEstimator.h" +#include + #include #include #include @@ -68,7 +70,11 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, m_robotToCamera(robotToCamera), lastPose(frc::Pose3d()), referencePose(frc::Pose3d()), - poseCacheTimestamp(-1_s) {} + poseCacheTimestamp(-1_s) { + HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, + InstanceCount); + InstanceCount++; +} PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, PoseStrategy strat, PhotonCamera&& cam, @@ -79,7 +85,11 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags, m_robotToCamera(robotToCamera), lastPose(frc::Pose3d()), referencePose(frc::Pose3d()), - poseCacheTimestamp(-1_s) {} + poseCacheTimestamp(-1_s) { + HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator, + InstanceCount); + InstanceCount++; +} void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) { if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR || diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 582458e014..99aa52c61b 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -205,6 +205,7 @@ class PhotonCamera { private: units::second_t lastVersionCheckTime = 0_s; inline static bool VERSION_CHECK_ENABLED = true; + inline static int InstanceCount = 0; void VerifyVersion(); }; diff --git a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index cdb60f6cee..2c863330bb 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -231,6 +231,8 @@ class PhotonPoseEstimator { units::second_t poseCacheTimestamp; + inline static int InstanceCount = 0; + inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; } std::optional Update(