001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.targeting.proto;
019
020import edu.wpi.first.math.geometry.Transform3d;
021import edu.wpi.first.util.protobuf.Protobuf;
022import java.util.ArrayList;
023import java.util.List;
024import org.photonvision.proto.Photon.ProtobufPhotonTrackedTarget;
025import org.photonvision.targeting.PhotonTrackedTarget;
026import org.photonvision.targeting.TargetCorner;
027import us.hebi.quickbuf.Descriptors.Descriptor;
028import us.hebi.quickbuf.RepeatedMessage;
029
030public class PhotonTrackedTargetProto
031        implements Protobuf<PhotonTrackedTarget, ProtobufPhotonTrackedTarget> {
032    @Override
033    public Class<PhotonTrackedTarget> getTypeClass() {
034        return PhotonTrackedTarget.class;
035    }
036
037    @Override
038    public Descriptor getDescriptor() {
039        return ProtobufPhotonTrackedTarget.getDescriptor();
040    }
041
042    @Override
043    public ProtobufPhotonTrackedTarget createMessage() {
044        return ProtobufPhotonTrackedTarget.newInstance();
045    }
046
047    @Override
048    public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) {
049        return new PhotonTrackedTarget(
050                msg.getYaw(),
051                msg.getPitch(),
052                msg.getArea(),
053                msg.getSkew(),
054                msg.getFiducialId(),
055                msg.getObjDetectionId(),
056                msg.getObjDetectionConf(),
057                Transform3d.proto.unpack(msg.getBestCameraToTarget()),
058                Transform3d.proto.unpack(msg.getAltCameraToTarget()),
059                msg.getPoseAmbiguity(),
060                TargetCorner.proto.unpack(msg.getMinAreaRectCorners()),
061                TargetCorner.proto.unpack(msg.getDetectedCorners()));
062    }
063
064    public List<PhotonTrackedTarget> unpack(RepeatedMessage<ProtobufPhotonTrackedTarget> msg) {
065        ArrayList<PhotonTrackedTarget> targets = new ArrayList<>(msg.length());
066        for (ProtobufPhotonTrackedTarget target : msg) {
067            targets.add(unpack(target));
068        }
069        return targets;
070    }
071
072    @Override
073    public void pack(ProtobufPhotonTrackedTarget msg, PhotonTrackedTarget value) {
074        msg.setYaw(value.getYaw())
075                .setPitch(value.getPitch())
076                .setSkew(value.getSkew())
077                .setArea(value.getArea())
078                .setFiducialId(value.getFiducialId())
079                .setPoseAmbiguity(value.getPoseAmbiguity())
080                .setObjDetectionConf(value.getDetectedObjectConfidence())
081                .setObjDetectionId(value.getDetectedObjectClassID());
082
083        Transform3d.proto.pack(msg.getMutableBestCameraToTarget(), value.getBestCameraToTarget());
084        Transform3d.proto.pack(msg.getMutableAltCameraToTarget(), value.getAlternateCameraToTarget());
085
086        TargetCorner.proto.pack(msg.getMutableMinAreaRectCorners(), value.getMinAreaRectCorners());
087        TargetCorner.proto.pack(msg.getMutableDetectedCorners(), value.getDetectedCorners());
088    }
089
090    public void pack(
091            RepeatedMessage<ProtobufPhotonTrackedTarget> msg, List<PhotonTrackedTarget> value) {
092        var targets = msg.reserve(value.size());
093        for (PhotonTrackedTarget trackedTarget : value) {
094            var target = targets.next();
095            pack(target, trackedTarget);
096        }
097    }
098}