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;
019
020import edu.wpi.first.util.protobuf.ProtobufSerializable;
021import java.util.ArrayList;
022import java.util.List;
023import java.util.Optional;
024import org.photonvision.common.dataflow.structures.PacketSerde;
025import org.photonvision.struct.PhotonPipelineResultSerde;
026import org.photonvision.targeting.proto.PhotonPipelineResultProto;
027import org.photonvision.targeting.serde.PhotonStructSerializable;
028
029/** Represents a pipeline result from a PhotonCamera. */
030public class PhotonPipelineResult
031        implements ProtobufSerializable, PhotonStructSerializable<PhotonPipelineResult> {
032    private static boolean HAS_WARNED = false;
033
034    // Frame capture metadata
035    public PhotonPipelineMetadata metadata;
036
037    // Targets to store.
038    public List<PhotonTrackedTarget> targets = new ArrayList<>();
039
040    // Multi-tag result
041    public Optional<MultiTargetPNPResult> multitagResult;
042
043    /** Constructs an empty pipeline result. */
044    public PhotonPipelineResult() {
045        this(new PhotonPipelineMetadata(), List.of(), Optional.empty());
046    }
047
048    /**
049     * Constructs a pipeline result.
050     *
051     * @param sequenceID The number of frames processed by this camera since boot
052     * @param captureTimestampMicros The time, in uS in the coprocessor's timebase, that the
053     *     coprocessor captured the image this result contains the targeting info of
054     * @param publishTimestampMicros The time, in uS in the coprocessor's timebase, that the
055     *     coprocessor published targeting info
056     * @param targets The list of targets identified by the pipeline.
057     */
058    public PhotonPipelineResult(
059            long sequenceID,
060            long captureTimestampMicros,
061            long publishTimestampMicros,
062            long timeSinceLastPong,
063            List<PhotonTrackedTarget> targets) {
064        this(
065                new PhotonPipelineMetadata(
066                        captureTimestampMicros, publishTimestampMicros, sequenceID, timeSinceLastPong),
067                targets,
068                Optional.empty());
069    }
070
071    /**
072     * Constructs a pipeline result.
073     *
074     * @param sequenceID The number of frames processed by this camera since boot
075     * @param captureTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor
076     *     captured the image this result contains the targeting info of
077     * @param publishTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor
078     *     published targeting info
079     * @param targets The list of targets identified by the pipeline.
080     * @param result Result from multi-target PNP.
081     */
082    public PhotonPipelineResult(
083            long sequenceID,
084            long captureTimestamp,
085            long publishTimestamp,
086            long timeSinceLastPong,
087            List<PhotonTrackedTarget> targets,
088            Optional<MultiTargetPNPResult> result) {
089        this(
090                new PhotonPipelineMetadata(
091                        captureTimestamp, publishTimestamp, sequenceID, timeSinceLastPong),
092                targets,
093                result);
094    }
095
096    public PhotonPipelineResult(
097            PhotonPipelineMetadata metadata,
098            List<PhotonTrackedTarget> targets,
099            Optional<MultiTargetPNPResult> result) {
100        this.metadata = metadata;
101        this.targets.addAll(targets);
102        this.multitagResult = result;
103    }
104
105    /**
106     * Returns the size of the packet needed to store this pipeline result.
107     *
108     * @return The size of the packet needed to store this pipeline result.
109     */
110    public int getPacketSize() {
111        throw new RuntimeException("TODO");
112        // return Double.BYTES // latency
113        //         + 1 // target count
114        //         + targets.size() * PhotonTrackedTarget.serde.getMaxByteSize()
115        //         + MultiTargetPNPResult.serde.getMaxByteSize();
116    }
117
118    /**
119     * Returns the best target in this pipeline result. If there are no targets, this method will
120     * return null. The best target is determined by the target sort mode in the PhotonVision UI.
121     *
122     * @return The best target of the pipeline result.
123     */
124    public PhotonTrackedTarget getBestTarget() {
125        if (!hasTargets() && !HAS_WARNED) {
126            String errStr =
127                    "This PhotonPipelineResult object has no targets associated with it! Please check hasTargets() "
128                            + "before calling this method. For more information, please review the PhotonLib "
129                            + "documentation at https://docs.photonvision.org";
130            System.err.println(errStr);
131            new Exception().printStackTrace();
132            HAS_WARNED = true;
133        }
134        return hasTargets() ? targets.get(0) : null;
135    }
136
137    /**
138     * Returns whether the pipeline has targets.
139     *
140     * @return Whether the pipeline has targets.
141     */
142    public boolean hasTargets() {
143        return !targets.isEmpty();
144    }
145
146    /**
147     * Returns a copy of the vector of targets.
148     *
149     * <p>Returned in the order set by target sort mode.
150     *
151     * @return A copy of the vector of targets.
152     */
153    public List<PhotonTrackedTarget> getTargets() {
154        return new ArrayList<>(targets);
155    }
156
157    /**
158     * Return the latest multi-target result. Be sure to check
159     * getMultiTagResult().estimatedPose.isPresent before using the pose estimate!
160     */
161    public Optional<MultiTargetPNPResult> getMultiTagResult() {
162        return multitagResult;
163    }
164
165    /**
166     * Returns the estimated time the frame was taken, in the Time Sync Server's time base (nt::Now).
167     * This is calculated using the estimated offset between Time Sync Server time and local time. The
168     * robot shall run a server, so the offset shall be 0.
169     *
170     * @return The timestamp in seconds
171     */
172    public double getTimestampSeconds() {
173        return metadata.captureTimestampMicros / 1e6;
174    }
175
176    @Override
177    public String toString() {
178        return "PhotonPipelineResult [metadata="
179                + metadata
180                + ", targets="
181                + targets
182                + ", multitagResult="
183                + multitagResult
184                + "]";
185    }
186
187    @Override
188    public int hashCode() {
189        final int prime = 31;
190        int result = 1;
191        result = prime * result + ((metadata == null) ? 0 : metadata.hashCode());
192        result = prime * result + ((targets == null) ? 0 : targets.hashCode());
193        result = prime * result + ((multitagResult == null) ? 0 : multitagResult.hashCode());
194        return result;
195    }
196
197    @Override
198    public boolean equals(Object obj) {
199        if (this == obj) return true;
200        if (obj == null) return false;
201        if (getClass() != obj.getClass()) return false;
202        PhotonPipelineResult other = (PhotonPipelineResult) obj;
203        if (metadata == null) {
204            if (other.metadata != null) return false;
205        } else if (!metadata.equals(other.metadata)) return false;
206        if (targets == null) {
207            if (other.targets != null) return false;
208        } else if (!targets.equals(other.targets)) return false;
209        if (multitagResult == null) {
210            if (other.multitagResult != null) return false;
211        } else if (!multitagResult.equals(other.multitagResult)) return false;
212        return true;
213    }
214
215    public static final PhotonPipelineResultSerde photonStruct = new PhotonPipelineResultSerde();
216    public static final PhotonPipelineResultProto proto = new PhotonPipelineResultProto();
217
218    @Override
219    public PacketSerde<PhotonPipelineResult> getSerde() {
220        return photonStruct;
221    }
222}