Source code

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.vision.processes;
019
020import java.util.ArrayList;
021import java.util.List;
022import java.util.Map;
023import java.util.concurrent.locks.ReentrantLock;
024import org.apache.commons.lang3.tuple.Pair;
025import org.opencv.core.Point;
026import org.photonvision.common.dataflow.DataChangeSubscriber;
027import org.photonvision.common.dataflow.events.DataChangeEvent;
028import org.photonvision.common.dataflow.events.IncomingWebSocketEvent;
029import org.photonvision.common.logging.LogGroup;
030import org.photonvision.common.logging.Logger;
031import org.photonvision.common.util.file.JacksonUtils;
032import org.photonvision.common.util.numbers.DoubleCouple;
033import org.photonvision.common.util.numbers.IntegerCouple;
034import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
035import org.photonvision.vision.pipeline.AdvancedPipelineSettings;
036import org.photonvision.vision.pipeline.PipelineType;
037import org.photonvision.vision.pipeline.UICalibrationData;
038import org.photonvision.vision.target.RobotOffsetPointOperation;
039
040@SuppressWarnings("unchecked")
041public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
042    private final VisionModule parentModule;
043    private final Logger logger;
044    private List<VisionModuleChange<?>> settingChanges = new ArrayList<>();
045    private final ReentrantLock changeListLock = new ReentrantLock();
046
047    public VisionModuleChangeSubscriber(VisionModule parentModule) {
048        this.parentModule = parentModule;
049        logger =
050                new Logger(
051                        VisionModuleChangeSubscriber.class,
052                        parentModule.visionSource.getSettables().getConfiguration().nickname,
053                        LogGroup.VisionModule);
054    }
055
056    @Override
057    public void onDataChangeEvent(DataChangeEvent<?> event) {
058        if (event instanceof IncomingWebSocketEvent wsEvent) {
059            // Camera index -1 means a "multicast event" (i.e. the event is received by all cameras)
060            if (wsEvent.cameraUniqueName != null
061                    && wsEvent.cameraUniqueName.equals(parentModule.uniqueName())) {
062                logger.trace("Got PSC event - propName: " + wsEvent.propertyName);
063                changeListLock.lock();
064                try {
065                    getSettingChanges()
066                            .add(
067                                    new VisionModuleChange(
068                                            wsEvent.propertyName,
069                                            wsEvent.data,
070                                            parentModule.pipelineManager.getCurrentPipeline().getSettings(),
071                                            wsEvent.originContext));
072                } finally {
073                    changeListLock.unlock();
074                }
075            }
076        }
077    }
078
079    public List<VisionModuleChange<?>> getSettingChanges() {
080        return settingChanges;
081    }
082
083    public void processSettingChanges() {
084        // special case for non-PipelineSetting changes
085        changeListLock.lock();
086        try {
087            for (var change : settingChanges) {
088                var propName = change.getPropName();
089                var newPropValue = change.getNewPropValue();
090                var currentSettings = change.getCurrentSettings();
091                var originContext = change.getOriginContext();
092                boolean handled = true;
093                switch (propName) {
094                    case "pipelineName" -> {
095                        newPipelineNickname((String) newPropValue);
096                        continue;
097                    }
098                    case "newPipelineInfo" -> newPipelineInfo((Pair<String, PipelineType>) newPropValue);
099                    case "deleteCurrPipeline" -> deleteCurrPipeline();
100                    case "changePipeline" -> changePipeline((Integer) newPropValue);
101                    case "startCalibration" -> startCalibration((Map<String, Object>) newPropValue);
102                    case "saveInputSnapshot" -> parentModule.saveInputSnapshot();
103                    case "saveOutputSnapshot" -> parentModule.saveOutputSnapshot();
104                    case "takeCalSnapshot" -> parentModule.takeCalibrationSnapshot();
105                    case "duplicatePipeline" -> duplicatePipeline((Integer) newPropValue);
106                    case "calibrationUploaded" -> {
107                        if (newPropValue instanceof CameraCalibrationCoefficients newCal) {
108                            parentModule.addCalibrationToConfig(newCal);
109                        } else {
110                            logger.warn("Received invalid calibration data");
111                        }
112                    }
113                    case "robotOffsetPoint" -> {
114                        if (currentSettings instanceof AdvancedPipelineSettings curAdvSettings) {
115                            robotOffsetPoint(curAdvSettings, (Integer) newPropValue);
116                        }
117                    }
118                    case "changePipelineType" -> {
119                        parentModule.changePipelineType((Integer) newPropValue);
120                        parentModule.saveAndBroadcastAll();
121                    }
122                    case "isDriverMode" -> parentModule.setDriverMode((Boolean) newPropValue);
123                    default -> handled = false;
124                }
125
126                // special case for camera settables
127                if (propName.startsWith("camera")) {
128                    var propMethodName = "set" + propName.replace("camera", "");
129                    var methods = parentModule.visionSource.getSettables().getClass().getMethods();
130                    for (var method : methods) {
131                        if (method.getName().equalsIgnoreCase(propMethodName)) {
132                            try {
133                                method.invoke(parentModule.visionSource.getSettables(), newPropValue);
134                            } catch (Exception e) {
135                                logger.error("Failed to invoke camera settable method: " + method.getName(), e);
136                            }
137                        }
138                    }
139                }
140
141                if (!handled) {
142                    try {
143                        setProperty(currentSettings, propName, newPropValue);
144                        logger.trace("Set prop " + propName + " to value " + newPropValue);
145                    } catch (NoSuchFieldException | IllegalAccessException e) {
146                        logger.error(
147                                "Could not set prop "
148                                        + propName
149                                        + " with value "
150                                        + newPropValue
151                                        + " on "
152                                        + currentSettings
153                                        + " | "
154                                        + e.getClass().getSimpleName(),
155                                e);
156                    } catch (Exception e) {
157                        logger.error("Unknown exception when setting PSC prop!", e);
158                    }
159                }
160
161                parentModule.saveAndBroadcastSelective(originContext, propName, newPropValue);
162            }
163            getSettingChanges().clear();
164        } finally {
165            changeListLock.unlock();
166        }
167    }
168
169    public void newPipelineNickname(String newNickname) {
170        logger.info("Changing pipeline nickname to " + newNickname);
171        parentModule.pipelineManager.getCurrentPipelineSettings().pipelineNickname = newNickname;
172        parentModule.saveAndBroadcastAll();
173    }
174
175    public void newPipelineInfo(Pair<String, PipelineType> typeName) {
176        var type = typeName.getRight();
177        var name = typeName.getLeft();
178
179        logger.info("Adding a " + type + " pipeline with name " + name);
180
181        var addedSettings = parentModule.pipelineManager.addPipeline(type);
182        addedSettings.pipelineNickname = name;
183        parentModule.saveAndBroadcastAll();
184    }
185
186    public void deleteCurrPipeline() {
187        var indexToDelete = parentModule.pipelineManager.getRequestedIndex();
188        logger.info("Deleting current pipe at index " + indexToDelete);
189        int newIndex = parentModule.pipelineManager.removePipeline(indexToDelete);
190        parentModule.setPipeline(newIndex);
191        parentModule.saveAndBroadcastAll();
192    }
193
194    public void changePipeline(int index) {
195        if (index == parentModule.pipelineManager.getRequestedIndex()) {
196            logger.debug("Skipping pipeline change, index " + index + " already active");
197            return;
198        }
199        parentModule.setPipeline(index);
200        parentModule.saveAndBroadcastAll();
201    }
202
203    public void startCalibration(Map<String, Object> data) {
204        try {
205            var deserialized = JacksonUtils.deserialize(data, UICalibrationData.class);
206            parentModule.startCalibration(deserialized);
207            parentModule.saveAndBroadcastAll();
208        } catch (Exception e) {
209            logger.error("Error deserializing start-calibration request", e);
210        }
211    }
212
213    public void duplicatePipeline(int index) {
214        var newIndex = parentModule.pipelineManager.duplicatePipeline(index);
215        parentModule.setPipeline(newIndex);
216        parentModule.saveAndBroadcastAll();
217    }
218
219    public void robotOffsetPoint(AdvancedPipelineSettings curAdvSettings, int offsetIndex) {
220        RobotOffsetPointOperation offsetOperation = RobotOffsetPointOperation.fromIndex(offsetIndex);
221
222        var latestTarget = parentModule.lastPipelineResultBestTarget;
223        if (latestTarget == null) {
224            return;
225        }
226
227        var newPoint = latestTarget.getTargetOffsetPoint();
228        switch (curAdvSettings.offsetRobotOffsetMode) {
229            case Single -> {
230                switch (offsetOperation) {
231                    case CLEAR -> curAdvSettings.offsetSinglePoint = new Point();
232                    case TAKE_SINGLE -> curAdvSettings.offsetSinglePoint = newPoint;
233                    case TAKE_FIRST_DUAL, TAKE_SECOND_DUAL -> {
234                        logger.warn("Dual point operation in single point mode");
235                    }
236                }
237            }
238            case Dual -> {
239                switch (offsetOperation) {
240                    case CLEAR -> {
241                        curAdvSettings.offsetDualPointA = new Point();
242                        curAdvSettings.offsetDualPointAArea = 0;
243                        curAdvSettings.offsetDualPointB = new Point();
244                        curAdvSettings.offsetDualPointBArea = 0;
245                    }
246                    case TAKE_FIRST_DUAL -> {
247                        // update point and area
248                        curAdvSettings.offsetDualPointA = newPoint;
249                        curAdvSettings.offsetDualPointAArea = latestTarget.getArea();
250                    }
251                    case TAKE_SECOND_DUAL -> {
252                        // update point and area
253                        curAdvSettings.offsetDualPointB = newPoint;
254                        curAdvSettings.offsetDualPointBArea = latestTarget.getArea();
255                    }
256                    case TAKE_SINGLE -> {
257                        logger.warn("Single point operation in dual point mode");
258                    }
259                }
260            }
261            case None -> {
262                logger.warn("Robot offset point operation requested, but no offset mode set");
263            }
264        }
265    }
266
267    /**
268     * Sets the value of a property in the given object using reflection. This method should not be
269     * used generally and is only known to be correct in the context of `onDataChangeEvent`.
270     *
271     * @param currentSettings The object whose property needs to be set.
272     * @param propName The name of the property to be set.
273     * @param newPropValue The new value to be assigned to the property.
274     * @throws IllegalAccessException If the field cannot be accessed.
275     * @throws NoSuchFieldException If the field does not exist.
276     * @throws Exception If an some other unknown exception occurs while setting the property.
277     */
278    protected static void setProperty(Object currentSettings, String propName, Object newPropValue)
279            throws IllegalAccessException, NoSuchFieldException, Exception {
280        var propField = currentSettings.getClass().getField(propName);
281        var propType = propField.getType();
282
283        if (propType.isEnum()) {
284            var actual = propType.getEnumConstants()[(int) newPropValue];
285            propField.set(currentSettings, actual);
286        } else if (propType.isAssignableFrom(DoubleCouple.class)) {
287            var orig = (ArrayList<Number>) newPropValue;
288            var actual = new DoubleCouple(orig.get(0), orig.get(1));
289            propField.set(currentSettings, actual);
290        } else if (propType.isAssignableFrom(IntegerCouple.class)) {
291            var orig = (ArrayList<Number>) newPropValue;
292            var actual = new IntegerCouple(orig.get(0).intValue(), orig.get(1).intValue());
293            propField.set(currentSettings, actual);
294        } else if (propType.equals(Double.TYPE)) {
295            propField.setDouble(currentSettings, ((Number) newPropValue).doubleValue());
296        } else if (propType.equals(Integer.TYPE)) {
297            propField.setInt(currentSettings, (Integer) newPropValue);
298        } else if (propType.equals(Boolean.TYPE)) {
299            if (newPropValue instanceof Integer intValue) {
300                propField.setBoolean(currentSettings, intValue != 0);
301            } else {
302                propField.setBoolean(currentSettings, (Boolean) newPropValue);
303            }
304        } else {
305            propField.set(currentSettings, newPropValue);
306        }
307    }
308}