From bafdd34b6da0b7f96be05ef1afc0843314fe080b Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Sat, 12 Oct 2024 15:26:06 +0800 Subject: [PATCH] removed legacy of maple config file --- .../apriltags/PhotonCameraProperties.java | 106 ------- .../utils/CustomConfigs/MapleConfigFile.java | 277 ------------------ .../MapleInterpolationTable.java | 41 --- .../robot/utils/MapleShooterOptimization.java | 17 -- 4 files changed, 441 deletions(-) delete mode 100644 src/main/java/frc/robot/utils/CustomConfigs/MapleConfigFile.java diff --git a/src/main/java/frc/robot/subsystems/vision/apriltags/PhotonCameraProperties.java b/src/main/java/frc/robot/subsystems/vision/apriltags/PhotonCameraProperties.java index 9cdd2ee..5e73dde 100644 --- a/src/main/java/frc/robot/subsystems/vision/apriltags/PhotonCameraProperties.java +++ b/src/main/java/frc/robot/subsystems/vision/apriltags/PhotonCameraProperties.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.utils.CustomConfigs.MapleConfigFile; import org.photonvision.simulation.SimCameraProperties; import java.io.IOException; @@ -77,111 +76,6 @@ public SimCameraProperties getSimulationProperties() { return cameraProperties; } - public static List loadCamerasPropertiesFromConfig(String configName) { - final MapleConfigFile configFile; - try { - configFile = MapleConfigFile.fromDeployedConfig("PhotonCamerasProperties", configName); - } catch (IOException e) { - DriverStation.reportError( - "cannot find camera properties config file: " - + configName - + " from deploy directory", - true); - return new ArrayList<>(); - } - return loadCamerasPropertiesFromConfig(configFile); - } - - public static List loadCamerasPropertiesFromConfig(MapleConfigFile configFile) { - MapleConfigFile.ConfigBlock generalBlock = configFile.getBlock("GeneralInfo"); - final int cameraAmount = generalBlock.getIntConfig("camerasAmount"); - final double cameraFPS = generalBlock.getDoubleConfig("cameraFPS"), - averageLatencyMS = generalBlock.getDoubleConfig("averageLatencyMS"), - latencyStandardDeviationMS = generalBlock.getDoubleConfig("latencyStandardDeviationMS"); - - final List cameraProperties = new ArrayList<>(cameraAmount); - - for (int i = 0; i < cameraAmount ; i++) { - final MapleConfigFile.ConfigBlock cameraBlock = configFile.getBlock("Camera"+i); - cameraProperties.add(loadSingleCameraPropertyFromBlock( - cameraBlock, cameraFPS, averageLatencyMS, latencyStandardDeviationMS - )); - } - - return cameraProperties; - } - - private static PhotonCameraProperties loadSingleCameraPropertyFromBlock(MapleConfigFile.ConfigBlock cameraBlock, double cameraFPS, double averageLatencyMS, double latencyStandardDeviationMS) { - final Transform3d robotToCameraInstallation = new Transform3d( - new Translation3d( - cameraBlock.getDoubleConfig("mountPositionToRobotCenterForwardsMeters"), - cameraBlock.getDoubleConfig("mountPositionToRobotCenterLeftwardsMeters"), - cameraBlock.getDoubleConfig("mountHeightMeters") - ), - new Rotation3d( - 0, - -Math.toRadians(cameraBlock.getDoubleConfig("cameraPitchDegrees")), - Math.toRadians(cameraBlock.getDoubleConfig("cameraYawDegrees")) - )); - final double cameraRollDeg = switch (cameraBlock.getIntConfig("captureOrientation")) { - case 1 -> 180; - case 2 -> -90; - case 3 -> 90; - default -> 0; - }; - final Transform3d cameraInstallationToCapturedImage = new Transform3d( - new Translation3d(), - new Rotation3d( - Math.toRadians(cameraRollDeg), - 0, 0 - )); - return new PhotonCameraProperties( - cameraBlock.getStringConfig("name"), - cameraFPS, - averageLatencyMS, - latencyStandardDeviationMS, - Rotation2d.fromDegrees(cameraBlock.getDoubleConfig("cameraFOVDegrees")), - cameraBlock.getDoubleConfig("calibrationAverageErrorPixel"), - cameraBlock.getDoubleConfig("calibrationErrorStandardDeviationPixel"), - cameraBlock.getIntConfig("captureWidthPixels"), - cameraBlock.getIntConfig("captureHeightPixels"), - robotToCameraInstallation.plus(cameraInstallationToCapturedImage) - ); - } - - public static MapleConfigFile createConfigFileForCameras(String name, int camerasAmount, double cameraFPS, double averageLatencyMS, double latencyStandardDeviationMS, int cameraWidthPixels, int cameraHeightPixels, double defaultCameraFOVDegrees) { - final MapleConfigFile configFile = new MapleConfigFile("PhotonCamerasProperties", name); - final MapleConfigFile.ConfigBlock generalBlock = configFile.getBlock("GeneralInfo"); - - generalBlock.putIntConfig("camerasAmount", camerasAmount); - generalBlock.putDoubleConfig("cameraFPS", cameraFPS); - generalBlock.putDoubleConfig("averageLatencyMS", averageLatencyMS); - generalBlock.putDoubleConfig("latencyStandardDeviationMS", latencyStandardDeviationMS); - - for (int i = 0; i < camerasAmount; i++) { - createConfigBlockForCamera( - configFile.getBlock("Camera"+i), - cameraWidthPixels,cameraHeightPixels,defaultCameraFOVDegrees - ); - } - - return configFile; - } - - private static void createConfigBlockForCamera(MapleConfigFile.ConfigBlock cameraBlock, int cameraWidthPixels, int cameraHeightPixels, double cameraFOVDegrees) { - cameraBlock.putStringConfig("name", "YOUR_CAMERA_NAME"); - cameraBlock.putIntConfig("captureWidthPixels", cameraWidthPixels); - cameraBlock.putIntConfig("captureHeightPixels", cameraHeightPixels); - cameraBlock.putDoubleConfig("cameraFOVDegrees", cameraFOVDegrees); - cameraBlock.putDoubleConfig("calibrationAverageErrorPixel", 0.35); - cameraBlock.putDoubleConfig("calibrationErrorStandardDeviationPixel", 0.1); - cameraBlock.putDoubleConfig("mountPositionToRobotCenterForwardsMeters", 0); - cameraBlock.putDoubleConfig("mountPositionToRobotCenterLeftwardsMeters", 0); - cameraBlock.putDoubleConfig("mountHeightMeters", 0); - cameraBlock.putDoubleConfig("cameraPitchDegrees", 0); - cameraBlock.putDoubleConfig("cameraYawDegrees", 0); - } - @Override public String toString() { return String.format(""" diff --git a/src/main/java/frc/robot/utils/CustomConfigs/MapleConfigFile.java b/src/main/java/frc/robot/utils/CustomConfigs/MapleConfigFile.java deleted file mode 100644 index 580a0a3..0000000 --- a/src/main/java/frc/robot/utils/CustomConfigs/MapleConfigFile.java +++ /dev/null @@ -1,277 +0,0 @@ -// By 5516 Iron Maple https://github.com/Shenzhen-Robotics-Alliance/ and ChatGPT -package frc.robot.utils.CustomConfigs; - -import edu.wpi.first.wpilibj.Filesystem; - -import java.io.File; -import java.io.FileWriter; -import java.io.IOException; -import java.nio.file.Path; -import java.nio.file.Paths; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; -import javax.xml.parsers.DocumentBuilder; -import javax.xml.parsers.DocumentBuilderFactory; - -import org.w3c.dom.Document; -import org.w3c.dom.Element; -import org.w3c.dom.Node; -import org.w3c.dom.NodeList; - -public class MapleConfigFile { - public final String configType; - public final String configName; - - public static final class ConfigBlock { - private final String blockName; - private final Map doubleConfigs = new HashMap<>(); - private final Map intConfigs = new HashMap<>(); - private final Map stringConfigs = new HashMap<>(); - - private final List configOrders = new ArrayList<>(); - - private ConfigBlock(String blockName) { - this.blockName = blockName; - } - - public boolean hasDoubleConfig(String name) { - return doubleConfigs.containsKey(name); - } - - public boolean hasIntConfig(String name) { - return intConfigs.containsKey(name); - } - - public boolean hasStringConfig(String name) { - return stringConfigs.containsKey(name); - } - public double getDoubleConfig(String name) throws NullPointerException { - if (!hasDoubleConfig(name)) - throw new NullPointerException( - "Configuration not found for block: " + blockName + ", config: " + name + ", type: double"); - return doubleConfigs.get(name); - } - - public int getIntConfig(String name) throws NullPointerException { - if (!hasIntConfig(name)) - throw new NullPointerException( - "Configuration not found for block: " + blockName + ", config: " + name + ", type: int"); - return intConfigs.get(name); - } - - public String getStringConfig(String name) throws NullPointerException { - if (!hasStringConfig(name)) - throw new NullPointerException( - "Configuration not found for block: " + blockName + ", config: " + name + ", type: string"); - return stringConfigs.get(name); - } - - public void putDoubleConfig(String configName, double value) throws IllegalArgumentException { - if (intConfigs.containsKey(configName) || stringConfigs.containsKey(configName)) - throw new IllegalArgumentException( - "Cannot put double config '" - + configName - + "' to block '" - + blockName - + "' since there is already an int or string config with the same name"); - configOrders.add(configName); - doubleConfigs.put(configName, value); - } - - public void putIntConfig(String configName, int value) throws IllegalArgumentException { - if (doubleConfigs.containsKey(configName) || stringConfigs.containsKey(configName)) - throw new IllegalArgumentException( - "Cannot put int config '" - + configName - + "' to block '" - + blockName - + "' since there is already a double or string config with the same name"); - configOrders.add(configName); - intConfigs.put(configName, value); - } - - public void putStringConfig(String configName, String value) throws IllegalArgumentException { - if (doubleConfigs.containsKey(configName) || intConfigs.containsKey(configName)) - throw new IllegalArgumentException( - "Cannot put string config '" - + configName - + "' to block '" - + blockName - + "' since there is already a double or int config with the same name"); - configOrders.add(configName); - stringConfigs.put(configName, value); - } - - public String getBlockName() { - return this.blockName; - } - } - - private final Map configBlocks = new HashMap<>(); - private final List configBlocksOrder = new ArrayList<>(); - - public MapleConfigFile(String configType, String configName) { - this.configType = configType; - this.configName = configName; - } - - public ConfigBlock getBlock(String blockName) { - if (!configBlocksOrder.contains(blockName)) { - configBlocks.put(blockName, new ConfigBlock(blockName)); - configBlocksOrder.add(blockName); - } - return configBlocks.get(blockName); - } - - public static MapleConfigFile fromDeployedConfig(String configType, String configName) - throws IllegalArgumentException, IOException { - MapleConfigFile configFile = new MapleConfigFile(configType, configName); - - final Path xmlFilePath = - Paths.get( - Filesystem.getDeployDirectory().getPath(), "configs", configType, configName + ".xml"); - File xmlFile = xmlFilePath.toFile(); - if (!xmlFile.exists()) { - throw new IOException("Config file does not exist: " + xmlFilePath); - } - - try { - DocumentBuilderFactory dbFactory = DocumentBuilderFactory.newInstance(); - DocumentBuilder dBuilder = dbFactory.newDocumentBuilder(); - Document doc = dBuilder.parse(xmlFile); - doc.getDocumentElement().normalize(); - - if (!doc.getDocumentElement().getNodeName().equals(configType)) { - throw new IllegalArgumentException("Root element is not " + configType); - } - - NodeList blocks = doc.getDocumentElement().getChildNodes(); - processBlocks(configFile, blocks); - } catch (Exception e) { - throw new IOException("Error reading config file", e); - } - - return configFile; - } - - private static void processBlocks(MapleConfigFile configFile, NodeList blocks) { - for (int i = 0; i < blocks.getLength(); i++) { - Node blockNode = blocks.item(i); - if (blockNode.getNodeType() == Node.ELEMENT_NODE) { - Element blockElement = (Element) blockNode; - String blockName = blockElement.getTagName(); - ConfigBlock block = new ConfigBlock(blockName); - readBlockConfig(blockElement, block); - configFile.configBlocks.put(blockName, block); - configFile.configBlocksOrder.add(blockName); - } - } - } - - private static void readBlockConfig(Element blockElement, ConfigBlock block) { - NodeList configNodes = blockElement.getChildNodes(); - for (int j = 0; j < configNodes.getLength(); j++) { - Node configNode = configNodes.item(j); - if (configNode.getNodeType() == Node.ELEMENT_NODE) { - addConfigToBlock((Element) configNode, block); - } - } - } - - private static void addConfigToBlock(Element configElement, ConfigBlock block) { - String configTag = configElement.getTagName(); - String type = configElement.getAttribute("type"); - String value = configElement.getTextContent(); - - switch (type) { - case "double": - block.putDoubleConfig(configTag, Double.parseDouble(value)); - break; - case "int": - block.putIntConfig(configTag, Integer.parseInt(value)); - break; - case "string": - block.putStringConfig(configTag, value); - break; - } - } - - public void saveConfigToUSBSafe() { - try { - saveConfigToUSB(); - } catch (IOException ignored) { - } - } - - public void saveConfigToUSB() throws IOException { - File usbDir = new File("/U/"); - if (!usbDir.exists()) { - throw new IOException("No USB connected"); - } - File configDir = new File(usbDir, "savedConfigs/" + this.configType); - if (!configDir.exists() && !configDir.mkdirs()) { - throw new IOException("Failed to create config directory on USB"); - } - File configFile = new File(configDir, this.configName + ".xml"); - try (FileWriter writer = new FileWriter(configFile)) { - writer.write("<" + this.configType + ">\n"); - writeAllConfigBlocks(this, writer); - writer.write("\n"); - } - } - - private static void writeAllConfigBlocks(MapleConfigFile config, FileWriter writer) throws IOException { - for (String blockName : config.configBlocksOrder) { - ConfigBlock block = config.configBlocks.get(blockName); - writer.write(" <" + block.blockName + ">\n"); - writeSingleConfigBlock(block, writer); - writer.write(" \n"); - } - } - - private static void writeSingleConfigBlock(ConfigBlock block, FileWriter writer) throws IOException { - for (String configName:block.configOrders) { - if (block.hasStringConfig(configName)) - writeStringConfig(block, configName, writer); - else if (block.hasIntConfig(configName)) - writeIntConfig(block, configName, writer); - else if (block.hasDoubleConfig(configName)) - writeDoubleConfig(block, configName, writer); - } - } - - private static void writeDoubleConfig(ConfigBlock block, String configName, FileWriter writer) throws IOException { - writer.write(" <" - + configName - + " type=\"double\">" - + block.getDoubleConfig(configName) - + "\n" - ); - } - - private static void writeStringConfig(ConfigBlock block, String configName, FileWriter writer) throws IOException { - writer.write(" <" - + configName - + " type=\"string\">" - + block.getStringConfig(configName) - + "\n" - ); - } - - private static void writeIntConfig(ConfigBlock block, String configName, FileWriter writer) throws IOException { - writer.write(" <" - + configName - + " type=\"int\">" - + block.getIntConfig(configName) - + "\n" - ); - } -} diff --git a/src/main/java/frc/robot/utils/CustomConfigs/MapleInterpolationTable.java b/src/main/java/frc/robot/utils/CustomConfigs/MapleInterpolationTable.java index a4010a4..e961743 100644 --- a/src/main/java/frc/robot/utils/CustomConfigs/MapleInterpolationTable.java +++ b/src/main/java/frc/robot/utils/CustomConfigs/MapleInterpolationTable.java @@ -43,21 +43,6 @@ public void updateValuesFromDashboard() { return; System.arraycopy(updatedValues, 0, values, 0, values.length); } - - public void toBlock(MapleConfigFile.ConfigBlock block) { - block.putStringConfig("name", variableName); - for (int i = 0; i < values.length; i++) - block.putDoubleConfig("sample"+i, values[i]); - } - - public static Variable fromBlock(MapleConfigFile.ConfigBlock block, int length) { - final String name = block.getStringConfig("name"); - final double[] values = new double[length]; - for (int i = 0; i < length; i++) - values[i] = block.getDoubleConfig("sample"+i); - - return new Variable(name, values); - } } public MapleInterpolationTable(String name, Variable independentVariable, Variable... interpolatedVariables) { this.tableName = name; @@ -120,30 +105,4 @@ public double findDerivative(String interpolatedVariableName, double independent return dy / dx; } - - public MapleConfigFile toConfigFile(String configType) { - final MapleConfigFile configFile = new MapleConfigFile(configType, tableName); - - final MapleConfigFile.ConfigBlock infoBlock = configFile.getBlock("info"); - infoBlock.putIntConfig("interpolatedVariablesCount", interpolatedVariables.size()); - infoBlock.putIntConfig("length", independentVariable.values.length); - independentVariable.toBlock(configFile.getBlock("variable0")); - int i = 1; - for (Variable interpolatedVariable:interpolatedVariables.values()) - interpolatedVariable.toBlock(configFile.getBlock("variable" + (i++))); - - return configFile; - } - - public static MapleInterpolationTable fromConfigFile(MapleConfigFile configFile) { - final MapleConfigFile.ConfigBlock infoBlock = configFile.getBlock("info"); - final int interpolatedVariablesCount = infoBlock.getIntConfig("interpolatedVariablesCount"), - length = infoBlock.getIntConfig("length"); - - final Variable independentVariable = Variable.fromBlock(configFile.getBlock("variable0"), length); - final Variable[] interpolateVariables = new Variable[interpolatedVariablesCount]; - for (int i = 0; i < interpolatedVariablesCount; i++) - interpolateVariables[i] = Variable.fromBlock(configFile.getBlock("variable"+(i+1)), length); - return new MapleInterpolationTable(configFile.configName, independentVariable, interpolateVariables); // TODO: write this part - } } diff --git a/src/main/java/frc/robot/utils/MapleShooterOptimization.java b/src/main/java/frc/robot/utils/MapleShooterOptimization.java index da85bc1..708df52 100644 --- a/src/main/java/frc/robot/utils/MapleShooterOptimization.java +++ b/src/main/java/frc/robot/utils/MapleShooterOptimization.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.HolonomicDriveSubsystem; -import frc.robot.utils.CustomConfigs.MapleConfigFile; import frc.robot.utils.CustomConfigs.MapleInterpolationTable; import org.littletonrobotics.junction.Logger; @@ -69,11 +68,6 @@ private MapleShooterOptimization(String name, MapleInterpolationTable table) { this.minShootingDistance = table.minX; this.maxShootingDistance = table.maxX; - - SmartDashboard.putData( - "InterpolationTables/" + table.tableName + "/SaveConfigsToUSB", - Commands.runOnce(this::saveConfigsToUSBIfExist) - ); } public double getFlightTimeSeconds(Translation2d targetPosition, Translation2d robotPosition) { @@ -124,17 +118,6 @@ public boolean isTargetInRange(Translation2d targetPosition, Translation2d robot return minShootingDistance <= distanceToTarget && maxShootingDistance >= distanceToTarget; } - public static MapleShooterOptimization fromDeployDirectory(String name) throws IOException { - final MapleInterpolationTable interpolationTable = MapleInterpolationTable.fromConfigFile( - MapleConfigFile.fromDeployedConfig("ShooterOptimization", name) - ); - return new MapleShooterOptimization(name, interpolationTable); - } - - public void saveConfigsToUSBIfExist() { - table.toConfigFile("ShooterOptimization").saveConfigToUSBSafe(); - } - public static class ChassisAimAtSpeakerDuringAuto extends Command { private final AtomicReference> rotationalTargetOverride; private final Supplier targetPositionSupplier;