-
Notifications
You must be signed in to change notification settings - Fork 196
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
5a4bf16
commit 9a7764f
Showing
20 changed files
with
838 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
68 changes: 68 additions & 0 deletions
68
...n-targeting/src/main/java/org/photonvision/targeting/proto/MultiTargetPNPResultProto.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,68 @@ | ||
| /* | ||
| * 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 <https://www.gnu.org/licenses/>. | ||
| */ | ||
|
|
||
| package org.photonvision.targeting.proto; | ||
|
|
||
| import edu.wpi.first.util.protobuf.Protobuf; | ||
| import java.util.Arrays; | ||
| import java.util.stream.Collectors; | ||
| import org.photonvision.proto.Photon.ProtobufMultiTargetPNPResult; | ||
| import org.photonvision.targeting.MultiTargetPNPResult; | ||
| import org.photonvision.targeting.PNPResult; | ||
| import us.hebi.quickbuf.Descriptors.Descriptor; | ||
| import us.hebi.quickbuf.RepeatedInt; | ||
|
|
||
| public class MultiTargetPNPResultProto | ||
| implements Protobuf<MultiTargetPNPResult, ProtobufMultiTargetPNPResult> { | ||
| @Override | ||
| public Class<MultiTargetPNPResult> getTypeClass() { | ||
| return MultiTargetPNPResult.class; | ||
| } | ||
|
|
||
| @Override | ||
| public Descriptor getDescriptor() { | ||
| return ProtobufMultiTargetPNPResult.getDescriptor(); | ||
| } | ||
|
|
||
| @Override | ||
| public Protobuf<?, ?>[] getNested() { | ||
| return new Protobuf<?, ?>[] {PNPResult.proto}; | ||
| } | ||
|
|
||
| @Override | ||
| public ProtobufMultiTargetPNPResult createMessage() { | ||
| return ProtobufMultiTargetPNPResult.newInstance(); | ||
| } | ||
|
|
||
| @Override | ||
| public MultiTargetPNPResult unpack(ProtobufMultiTargetPNPResult msg) { | ||
| return new MultiTargetPNPResult( | ||
| PNPResult.proto.unpack(msg.getEstimatedPose()), | ||
| // TODO better way of doing this | ||
| Arrays.stream(msg.getFiducialIdsUsed().array()).boxed().collect(Collectors.toList())); | ||
| } | ||
|
|
||
| @Override | ||
| public void pack(ProtobufMultiTargetPNPResult msg, MultiTargetPNPResult value) { | ||
| PNPResult.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose); | ||
|
|
||
| RepeatedInt idsUsed = msg.getMutableFiducialIdsUsed().reserve(value.fiducialIDsUsed.size()); | ||
| for (int i = 0; i < value.fiducialIDsUsed.size(); i++) { | ||
| idsUsed.add(value.fiducialIDsUsed.get(i)); | ||
| } | ||
| } | ||
| } |
70 changes: 70 additions & 0 deletions
70
photon-targeting/src/main/java/org/photonvision/targeting/proto/PNPResultProto.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,70 @@ | ||
| /* | ||
| * 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 <https://www.gnu.org/licenses/>. | ||
| */ | ||
|
|
||
| package org.photonvision.targeting.proto; | ||
|
|
||
| import edu.wpi.first.math.geometry.Transform3d; | ||
| import edu.wpi.first.util.protobuf.Protobuf; | ||
| import org.photonvision.proto.Photon.ProtobufPNPResult; | ||
| import org.photonvision.targeting.PNPResult; | ||
| import us.hebi.quickbuf.Descriptors.Descriptor; | ||
|
|
||
| public class PNPResultProto implements Protobuf<PNPResult, ProtobufPNPResult> { | ||
| @Override | ||
| public Class<PNPResult> getTypeClass() { | ||
| return PNPResult.class; | ||
| } | ||
|
|
||
| @Override | ||
| public Descriptor getDescriptor() { | ||
| return ProtobufPNPResult.getDescriptor(); | ||
| } | ||
|
|
||
| @Override | ||
| public Protobuf<?, ?>[] getNested() { | ||
| return new Protobuf<?, ?>[] {Transform3d.proto}; | ||
| } | ||
|
|
||
| @Override | ||
| public ProtobufPNPResult createMessage() { | ||
| return ProtobufPNPResult.newInstance(); | ||
| } | ||
|
|
||
| @Override | ||
| public PNPResult unpack(ProtobufPNPResult msg) { | ||
| if (!msg.getIsPresent()) { | ||
| return new PNPResult(); | ||
| } | ||
|
|
||
| return new PNPResult( | ||
| Transform3d.proto.unpack(msg.getBest()), | ||
| Transform3d.proto.unpack(msg.getAlt()), | ||
| msg.getAmbiguity(), | ||
| msg.getBestReprojErr(), | ||
| msg.getAltReprojErr()); | ||
| } | ||
|
|
||
| @Override | ||
| public void pack(ProtobufPNPResult msg, PNPResult value) { | ||
| Transform3d.proto.pack(msg.getMutableBest(), value.best); | ||
| Transform3d.proto.pack(msg.getMutableAlt(), value.alt); | ||
| msg.setAmbiguity(value.ambiguity) | ||
| .setBestReprojErr(value.bestReprojErr) | ||
| .setAltReprojErr(value.altReprojErr) | ||
| .setIsPresent(value.isPresent); | ||
| } | ||
| } |
64 changes: 64 additions & 0 deletions
64
...n-targeting/src/main/java/org/photonvision/targeting/proto/PhotonPipelineResultProto.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,64 @@ | ||
| /* | ||
| * 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 <https://www.gnu.org/licenses/>. | ||
| */ | ||
|
|
||
| package org.photonvision.targeting.proto; | ||
|
|
||
| import edu.wpi.first.util.protobuf.Protobuf; | ||
| import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult; | ||
| import org.photonvision.targeting.MultiTargetPNPResult; | ||
| import org.photonvision.targeting.PhotonPipelineResult; | ||
| import org.photonvision.targeting.PhotonTrackedTarget; | ||
| import us.hebi.quickbuf.Descriptors.Descriptor; | ||
|
|
||
| public class PhotonPipelineResultProto | ||
| implements Protobuf<PhotonPipelineResult, ProtobufPhotonPipelineResult> { | ||
| @Override | ||
| public Class<PhotonPipelineResult> getTypeClass() { | ||
| return PhotonPipelineResult.class; | ||
| } | ||
|
|
||
| @Override | ||
| public Descriptor getDescriptor() { | ||
| return ProtobufPhotonPipelineResult.getDescriptor(); | ||
| } | ||
|
|
||
| @Override | ||
| public Protobuf<?, ?>[] getNested() { | ||
| return new Protobuf<?, ?>[] {PhotonTrackedTarget.proto, MultiTargetPNPResult.proto}; | ||
| } | ||
|
|
||
| @Override | ||
| public ProtobufPhotonPipelineResult createMessage() { | ||
| return ProtobufPhotonPipelineResult.newInstance(); | ||
| } | ||
|
|
||
| @Override | ||
| public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { | ||
| return new PhotonPipelineResult( | ||
| msg.getLatencyMs(), | ||
| PhotonTrackedTarget.proto.unpack(msg.getTargets()), | ||
| MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult())); | ||
| } | ||
|
|
||
| @Override | ||
| public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) { | ||
| PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.getTargets()); | ||
| MultiTargetPNPResult.proto.pack(msg.getMutableMultiTargetResult(), value.getMultiTagResult()); | ||
|
|
||
| msg.setLatencyMs(value.getLatencyMillis()); | ||
| } | ||
| } |
99 changes: 99 additions & 0 deletions
99
...on-targeting/src/main/java/org/photonvision/targeting/proto/PhotonTrackedTargetProto.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,99 @@ | ||
| /* | ||
| * 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 <https://www.gnu.org/licenses/>. | ||
| */ | ||
|
|
||
| package org.photonvision.targeting.proto; | ||
|
|
||
| import edu.wpi.first.math.geometry.Transform3d; | ||
| import edu.wpi.first.util.protobuf.Protobuf; | ||
| import java.util.ArrayList; | ||
| import java.util.List; | ||
| import org.photonvision.proto.Photon.ProtobufPhotonTrackedTarget; | ||
| import org.photonvision.targeting.PhotonTrackedTarget; | ||
| import org.photonvision.targeting.TargetCorner; | ||
| import us.hebi.quickbuf.Descriptors.Descriptor; | ||
| import us.hebi.quickbuf.RepeatedMessage; | ||
|
|
||
| public class PhotonTrackedTargetProto | ||
| implements Protobuf<PhotonTrackedTarget, ProtobufPhotonTrackedTarget> { | ||
| @Override | ||
| public Class<PhotonTrackedTarget> getTypeClass() { | ||
| return PhotonTrackedTarget.class; | ||
| } | ||
|
|
||
| @Override | ||
| public Descriptor getDescriptor() { | ||
| return ProtobufPhotonTrackedTarget.getDescriptor(); | ||
| } | ||
|
|
||
| @Override | ||
| public Protobuf<?, ?>[] getNested() { | ||
| return new Protobuf<?, ?>[] {Transform3d.proto, TargetCorner.proto}; | ||
| } | ||
|
|
||
| @Override | ||
| public ProtobufPhotonTrackedTarget createMessage() { | ||
| return ProtobufPhotonTrackedTarget.newInstance(); | ||
| } | ||
|
|
||
| @Override | ||
| public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { | ||
| return new PhotonTrackedTarget( | ||
| msg.getYaw(), | ||
| msg.getPitch(), | ||
| msg.getArea(), | ||
| msg.getSkew(), | ||
| msg.getFiducialId(), | ||
| Transform3d.proto.unpack(msg.getBestCameraToTarget()), | ||
| Transform3d.proto.unpack(msg.getAltCameraToTarget()), | ||
| msg.getPoseAmbiguity(), | ||
| TargetCorner.proto.unpack(msg.getMinAreaRectCorners()), | ||
| TargetCorner.proto.unpack(msg.getDetectedCorners())); | ||
| } | ||
|
|
||
| public List<PhotonTrackedTarget> unpack(RepeatedMessage<ProtobufPhotonTrackedTarget> msg) { | ||
| ArrayList<PhotonTrackedTarget> targets = new ArrayList<>(msg.length()); | ||
| for (ProtobufPhotonTrackedTarget target : msg) { | ||
| targets.add(unpack(target)); | ||
| } | ||
| return targets; | ||
| } | ||
|
|
||
| @Override | ||
| public void pack(ProtobufPhotonTrackedTarget msg, PhotonTrackedTarget value) { | ||
| msg.setYaw(value.getYaw()) | ||
| .setPitch(value.getPitch()) | ||
| .setSkew(value.getSkew()) | ||
| .setArea(value.getArea()) | ||
| .setFiducialId(value.getFiducialId()) | ||
| .setPoseAmbiguity(value.getPoseAmbiguity()); | ||
|
|
||
| Transform3d.proto.pack(msg.getMutableBestCameraToTarget(), value.getBestCameraToTarget()); | ||
| Transform3d.proto.pack(msg.getMutableAltCameraToTarget(), value.getAlternateCameraToTarget()); | ||
|
|
||
| TargetCorner.proto.pack(msg.getMutableMinAreaRectCorners(), value.getMinAreaRectCorners()); | ||
| TargetCorner.proto.pack(msg.getMutableDetectedCorners(), value.getDetectedCorners()); | ||
| } | ||
|
|
||
| public void pack( | ||
| RepeatedMessage<ProtobufPhotonTrackedTarget> msg, List<PhotonTrackedTarget> value) { | ||
| var targets = msg.reserve(value.size()); | ||
| for (PhotonTrackedTarget trackedTarget : value) { | ||
| var target = targets.next(); | ||
| pack(target, trackedTarget); | ||
| } | ||
| } | ||
| } |
Oops, something went wrong.