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