From 91700ec0a7f4874e5a2dd4d734ce8950c797a0a5 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Sun, 14 Sep 2025 20:57:59 -0700 Subject: [PATCH 01/11] Get simulator working with MF3 --- .gitignore | 1 + simConfig.txt | 1 + .../com/team766/hal/mock/MockJoystick.java | 3 +- .../com/team766/hal/simulator/RobotMain.java | 91 +++++++------------ .../hal/simulator/SimulatorInterface.java | 10 ++ .../team766/hal/simulator/VrConnector.java | 28 ++++-- .../java/com/team766/hal/wpilib/Joystick.java | 2 +- .../com/team766/hal/wpilib/RobotMain.java | 84 ++++++++++++++++- .../com/team766/simulator/Parameters.java | 2 +- .../java/com/team766/simulator/Program.java | 7 -- .../team766/simulator/ProgramInterface.java | 2 - .../java/com/team766/simulator/Simulator.java | 38 ++++++-- 12 files changed, 177 insertions(+), 92 deletions(-) create mode 100644 src/main/java/com/team766/hal/simulator/SimulatorInterface.java delete mode 100644 src/main/java/com/team766/simulator/Program.java diff --git a/.gitignore b/.gitignore index 8385d4a5d..7437c1421 100644 --- a/.gitignore +++ b/.gitignore @@ -172,6 +172,7 @@ out/ # Simulation GUI and other tools window save file networktables.json simgui.json +simgui-ds.json *-window.json # Simulation data log directory diff --git a/simConfig.txt b/simConfig.txt index a3afa2071..6d76a7d03 100644 --- a/simConfig.txt +++ b/simConfig.txt @@ -1,4 +1,5 @@ { + "simulationMode": "VrConnector", "drive": { "leftMotor": { "deviceId": 6, diff --git a/src/main/java/com/team766/hal/mock/MockJoystick.java b/src/main/java/com/team766/hal/mock/MockJoystick.java index 03bb1aff2..31e968651 100755 --- a/src/main/java/com/team766/hal/mock/MockJoystick.java +++ b/src/main/java/com/team766/hal/mock/MockJoystick.java @@ -27,7 +27,8 @@ public double getAxis(final int axis) { @Override public boolean isAxisMoved(int axis) { - return getAxis(axis) >= axisDeadzoneMap.getOrDefault(axis, defaultAxisDeadzone); + return Math.abs(axisValues[axis]) + >= axisDeadzoneMap.getOrDefault(axis, defaultAxisDeadzone); } @Override diff --git a/src/main/java/com/team766/hal/simulator/RobotMain.java b/src/main/java/com/team766/hal/simulator/RobotMain.java index 5102c0cf0..b59a0da18 100755 --- a/src/main/java/com/team766/hal/simulator/RobotMain.java +++ b/src/main/java/com/team766/hal/simulator/RobotMain.java @@ -4,7 +4,6 @@ import com.team766.hal.GenericRobotMain; import com.team766.hal.RobotProvider; import com.team766.logging.LoggerExceptionUtils; -import com.team766.simulator.Program; import com.team766.simulator.ProgramInterface; import com.team766.simulator.Simulator; import java.io.IOException; @@ -16,7 +15,7 @@ enum Mode { } private GenericRobotMain robot; - private Runnable simulator; + private SimulatorInterface simulator; @SuppressWarnings("StaticAssignmentInConstructor") public RobotMain(final Mode mode) { @@ -28,59 +27,16 @@ public RobotMain(final Mode mode) { robot = new GenericRobotMain(); robot.robotInit(); - - ProgramInterface.program = - new Program() { - ProgramInterface.RobotMode prevRobotMode = null; - - @Override - public void step(double dt) { - switch (ProgramInterface.robotMode) { - case DISABLED: - if (prevRobotMode != ProgramInterface.RobotMode.DISABLED) { - robot.disabledInit(); - prevRobotMode = ProgramInterface.RobotMode.DISABLED; - } - robot.disabledPeriodic(); - break; - case AUTON: - if (prevRobotMode != ProgramInterface.RobotMode.AUTON) { - robot.autonomousInit(); - prevRobotMode = ProgramInterface.RobotMode.AUTON; - } - robot.autonomousPeriodic(); - break; - case TELEOP: - if (prevRobotMode != ProgramInterface.RobotMode.TELEOP) { - robot.teleopInit(); - prevRobotMode = ProgramInterface.RobotMode.TELEOP; - } - robot.teleopPeriodic(); - break; - default: - LoggerExceptionUtils.logException( - new IllegalArgumentException( - "Value of ProgramInterface.robotMode invalid. Provided value: " - + ProgramInterface.robotMode)); - break; - } - } - - @Override - public void reset() { - robot.resetAutonomousMode("simulation reset"); - } - }; } catch (Exception exc) { exc.printStackTrace(); LoggerExceptionUtils.logException(exc); } switch (mode) { - case MaroonSim: + case MaroonSim -> { simulator = new Simulator(); - break; - case VrConnector: + } + case VrConnector -> { ProgramInterface.robotMode = ProgramInterface.RobotMode.DISABLED; try { simulator = new VrConnector(); @@ -88,19 +44,42 @@ public void reset() { throw new RuntimeException( "Error initializing communication with 3d Simulator", ex); } - break; - default: - LoggerExceptionUtils.logException( - new IllegalArgumentException( - "Unknown simulator mode. ProgramInterface.robotMode: " - + ProgramInterface.robotMode)); - break; + } } + + simulator.setResetHandler(() -> robot.resetAutonomousMode("simulation reset")); } public void run() { try { - simulator.run(); + ProgramInterface.RobotMode prevRobotMode = null; + while (true) { + simulator.prepareStep(); + + switch (ProgramInterface.robotMode) { + case DISABLED -> { + if (prevRobotMode != ProgramInterface.RobotMode.DISABLED) { + robot.disabledInit(); + prevRobotMode = ProgramInterface.RobotMode.DISABLED; + } + robot.disabledPeriodic(); + } + case AUTON -> { + if (prevRobotMode != ProgramInterface.RobotMode.AUTON) { + robot.autonomousInit(); + prevRobotMode = ProgramInterface.RobotMode.AUTON; + } + robot.autonomousPeriodic(); + } + case TELEOP -> { + if (prevRobotMode != ProgramInterface.RobotMode.TELEOP) { + robot.teleopInit(); + prevRobotMode = ProgramInterface.RobotMode.TELEOP; + } + robot.teleopPeriodic(); + } + } + } } catch (Exception exc) { exc.printStackTrace(); LoggerExceptionUtils.logException(exc); diff --git a/src/main/java/com/team766/hal/simulator/SimulatorInterface.java b/src/main/java/com/team766/hal/simulator/SimulatorInterface.java new file mode 100644 index 000000000..4b1969d9c --- /dev/null +++ b/src/main/java/com/team766/hal/simulator/SimulatorInterface.java @@ -0,0 +1,10 @@ +package com.team766.hal.simulator; + +public interface SimulatorInterface { + /** + * @return deltaTime for the pending program step + */ + double prepareStep(); + + void setResetHandler(Runnable handler); +} diff --git a/src/main/java/com/team766/hal/simulator/VrConnector.java b/src/main/java/com/team766/hal/simulator/VrConnector.java index 2b60c1f2a..0962d0bb5 100644 --- a/src/main/java/com/team766/hal/simulator/VrConnector.java +++ b/src/main/java/com/team766/hal/simulator/VrConnector.java @@ -16,7 +16,7 @@ import java.util.List; import java.util.Map; -public class VrConnector implements Runnable { +public class VrConnector implements SimulatorInterface { private static class PortMapping { public final int messageDataIndex; public final int robotPortIndex; @@ -149,8 +149,11 @@ private static class CANPortMapping { private static final int feedbackPort = 7662; private static final int BUF_SZ = 1024; + private Runnable resetHandler; + private long startTime; private boolean started = false; + private double prevStepSimTime = 0; private Selector selector; private InetSocketAddress sendAddr; @@ -353,8 +356,8 @@ private boolean process() throws IOException { return newData; } - public void run() { - double prevSimTime = 0; + @Override + public double prepareStep() { while (true) { boolean newData = false; try { @@ -374,7 +377,9 @@ public void run() { } if (resetCounter != lastResetCounter) { lastResetCounter = resetCounter; - ProgramInterface.program.reset(); + if (resetHandler != null) { + resetHandler.run(); + } } if (!newData) { continue; @@ -390,13 +395,16 @@ public void run() { } else { continue; } - prevSimTime = ProgramInterface.simulationTime; - } - if (ProgramInterface.program != null) { - final double time = ProgramInterface.simulationTime; - ProgramInterface.program.step(time - prevSimTime); - prevSimTime = time; + prevStepSimTime = ProgramInterface.simulationTime; } + final double time = ProgramInterface.simulationTime; + prevStepSimTime = time; + return time - prevStepSimTime; } } + + @Override + public void setResetHandler(Runnable handler) { + resetHandler = handler; + } } diff --git a/src/main/java/com/team766/hal/wpilib/Joystick.java b/src/main/java/com/team766/hal/wpilib/Joystick.java index b0e57dad6..95deb5bd6 100755 --- a/src/main/java/com/team766/hal/wpilib/Joystick.java +++ b/src/main/java/com/team766/hal/wpilib/Joystick.java @@ -21,7 +21,7 @@ public double getAxis(final int axis) { @Override public boolean isAxisMoved(int axis) { - return java.lang.Math.abs(getAxis(axis)) + return Math.abs(getRawAxis(axis)) >= axisDeadzoneMap.getOrDefault(axis, defaultAxisDeadzone); } diff --git a/src/main/java/com/team766/hal/wpilib/RobotMain.java b/src/main/java/com/team766/hal/wpilib/RobotMain.java index b3ae9c349..b9c8a50de 100755 --- a/src/main/java/com/team766/hal/wpilib/RobotMain.java +++ b/src/main/java/com/team766/hal/wpilib/RobotMain.java @@ -5,15 +5,23 @@ import com.team766.hal.CanivPoller; import com.team766.hal.GenericRobotMain; import com.team766.hal.RobotProvider; +import com.team766.hal.simulator.SimulationRobotProvider; +import com.team766.hal.simulator.SimulatorInterface; +import com.team766.hal.simulator.VrConnector; +import com.team766.logging.Category; import com.team766.logging.LoggerExceptionUtils; +import com.team766.logging.Severity; +import com.team766.simulator.ProgramInterface; +import com.team766.simulator.Simulator; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; import java.io.File; -// import java.nio.file.Files; +import java.io.IOException; import java.nio.file.Path; import java.util.function.Supplier; import org.littletonrobotics.junction.LoggedRobot; @@ -26,6 +34,7 @@ public class RobotMain extends LoggedRobot { private static final String INTERNAL_CONFIG_FILE = "/home/lvuser/robotConfig.txt"; private GenericRobotMain robot; + private SimulatorInterface simulator; public static void main(final String... args) { Supplier supplier = @@ -94,9 +103,15 @@ public void robotInit() { configFromUSB = false; } - ConfigFileReader.instance = - new ConfigFileReader(filename, configFromUSB ? INTERNAL_CONFIG_FILE : null); - RobotProvider.instance = new WPIRobotProvider(); + if (isSimulation()) { + ConfigFileReader.instance = new ConfigFileReader("simConfig.txt"); + // TODO: Use WPILib's simulation interfaces and switch this to WPIRobotProvider + RobotProvider.instance = new SimulationRobotProvider(); + } else { + ConfigFileReader.instance = + new ConfigFileReader(filename, configFromUSB ? INTERNAL_CONFIG_FILE : null); + RobotProvider.instance = new WPIRobotProvider(); + } robot = new GenericRobotMain(); DriverStation.startDataLog(DataLogManager.getLog()); @@ -150,6 +165,67 @@ public void autonomousInit() { } } + @Override + public void simulationInit() { + try { + enum SimulationMode { + MaroonSim, + VrConnector, + } + final var simulationMode = + ConfigFileReader.getInstance().getEnum(SimulationMode.class, "simulationMode"); + switch (simulationMode.get()) { + case MaroonSim -> { + com.team766.logging.Logger.get(Category.FRAMEWORK) + .logRaw(Severity.INFO, "Running Maroon simulator"); + simulator = new Simulator(); + } + case VrConnector -> { + com.team766.logging.Logger.get(Category.FRAMEWORK) + .logRaw(Severity.INFO, "Running VR simulator"); + ProgramInterface.robotMode = ProgramInterface.RobotMode.DISABLED; + try { + simulator = new VrConnector(); + } catch (IOException ex) { + throw new RuntimeException( + "Error initializing communication with 3d Simulator", ex); + } + } + } + simulator.setResetHandler(() -> robot.resetAutonomousMode("simulation reset")); + } catch (Exception exc) { + exc.printStackTrace(); + LoggerExceptionUtils.logException(exc); + } + } + + @Override + public void simulationPeriodic() { + if (simulator == null) { + return; + } + try { + final double dt = simulator.prepareStep(); + switch (ProgramInterface.robotMode) { + case AUTON -> { + DriverStationSim.setAutonomous(true); + DriverStationSim.setEnabled(true); + } + case DISABLED -> { + DriverStationSim.setEnabled(false); + } + case TELEOP -> { + DriverStationSim.setAutonomous(false); + DriverStationSim.setEnabled(true); + } + } + DriverStationSim.notifyNewData(); + } catch (Exception exc) { + exc.printStackTrace(); + LoggerExceptionUtils.logException(exc); + } + } + @Override public void teleopInit() { try { diff --git a/src/main/java/com/team766/simulator/Parameters.java b/src/main/java/com/team766/simulator/Parameters.java index 6d3814cd9..e2d49cc1b 100644 --- a/src/main/java/com/team766/simulator/Parameters.java +++ b/src/main/java/com/team766/simulator/Parameters.java @@ -2,9 +2,9 @@ public class Parameters { public static final double TIME_STEP = 0.0001; // seconds - public static final double DURATION = 10.0; // seconds public static final double LOGGING_PERIOD = 0.005; // seconds + public static final double DISPLAY_PERIOD = 10.0; // seconds // Robot mode to run in the simulator public static final ProgramInterface.RobotMode INITIAL_ROBOT_MODE = diff --git a/src/main/java/com/team766/simulator/Program.java b/src/main/java/com/team766/simulator/Program.java deleted file mode 100644 index c6753fdef..000000000 --- a/src/main/java/com/team766/simulator/Program.java +++ /dev/null @@ -1,7 +0,0 @@ -package com.team766.simulator; - -public interface Program { - public void step(double dt); - - void reset(); -} diff --git a/src/main/java/com/team766/simulator/ProgramInterface.java b/src/main/java/com/team766/simulator/ProgramInterface.java index 15b293e32..6091e094f 100644 --- a/src/main/java/com/team766/simulator/ProgramInterface.java +++ b/src/main/java/com/team766/simulator/ProgramInterface.java @@ -5,8 +5,6 @@ import java.lang.reflect.Array; public class ProgramInterface { - public static Program program = null; - public static double simulationTime; public static int driverStationUpdateNumber = 0; diff --git a/src/main/java/com/team766/simulator/Simulator.java b/src/main/java/com/team766/simulator/Simulator.java index 05ab17bd1..cf46edb23 100644 --- a/src/main/java/com/team766/simulator/Simulator.java +++ b/src/main/java/com/team766/simulator/Simulator.java @@ -1,5 +1,6 @@ package com.team766.simulator; +import com.team766.hal.simulator.SimulatorInterface; import com.team766.simulator.elements.*; import com.team766.simulator.mechanisms.*; import com.team766.simulator.ui.*; @@ -8,15 +9,18 @@ import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder; -public class Simulator implements Runnable { +public class Simulator implements SimulatorInterface { private ElectricalSystem electricalSystem = new ElectricalSystem(); private PneumaticsSystem pneumaticsSystem = new PneumaticsSystem(); private WestCoastDrive drive = new WestCoastDrive(electricalSystem); private AirCompressor compressor = new AirCompressor(); private DoubleJointedArm arm = new DoubleJointedArm(electricalSystem); + private Runnable resetHandler; + private double time; private double nextLogTime; + private double nextDisplayTime = Parameters.DISPLAY_PERIOD; private final Metrics metrics = new Metrics(); private final Metrics.Series xPositionSeries = metrics.addSeries("X Position (m)", false); @@ -54,7 +58,8 @@ public Simulator() { } } - public void step() { + @Override + public double prepareStep() { double dt = Parameters.TIME_STEP; time += dt; ProgramInterface.simulationTime = time; @@ -64,8 +69,8 @@ public void step() { drive.step(dt); arm.step(dt); - if (ProgramInterface.program != null) { - ProgramInterface.program.step(dt); + if (Math.abs(time - 3.0) < Parameters.TIME_STEP) { + pneumaticsSystem.ventPressure(); } if (nextLogTime <= time) { @@ -116,19 +121,32 @@ public void step() { arm.getJ2Position().get(1, 0) }); } + + if (nextDisplayTime <= time) { + nextDisplayTime += Parameters.DISPLAY_PERIOD; + + displayResultsUi(); + } + + return dt; } - public void run() { + public void reset() { metrics.clear(); time = 0.0; nextLogTime = 0.0; - while (time <= Parameters.DURATION) { - step(); - if (Math.abs(time - 3.0) < Parameters.TIME_STEP) { - pneumaticsSystem.ventPressure(); - } + nextDisplayTime = Parameters.DISPLAY_PERIOD; + if (resetHandler != null) { + resetHandler.run(); } + } + + @Override + public void setResetHandler(Runnable handler) { + resetHandler = handler; + } + void displayResultsUi() { var playbackTimer = new PlaybackTimer(time); // var trajectoryPanel = new Trajectory(driveTrajectory, playbackTimer); From faa1086c17f76da44cb723435f9ede85b575b8c3 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Mon, 15 Sep 2025 07:01:34 +0000 Subject: [PATCH 02/11] gradle simulateJava shim --- .../com/team766/hal/simulator/RobotMain.java | 108 ++---------------- 1 file changed, 8 insertions(+), 100 deletions(-) diff --git a/src/main/java/com/team766/hal/simulator/RobotMain.java b/src/main/java/com/team766/hal/simulator/RobotMain.java index b59a0da18..2072f4764 100755 --- a/src/main/java/com/team766/hal/simulator/RobotMain.java +++ b/src/main/java/com/team766/hal/simulator/RobotMain.java @@ -1,109 +1,17 @@ package com.team766.hal.simulator; -import com.team766.config.ConfigFileReader; -import com.team766.hal.GenericRobotMain; -import com.team766.hal.RobotProvider; -import com.team766.logging.LoggerExceptionUtils; -import com.team766.simulator.ProgramInterface; -import com.team766.simulator.Simulator; import java.io.IOException; +// This is a backwards compatibility shim for existing 3d simulator packages. +// New applications should run `./gradlew simulateJava` directly. public class RobotMain { - enum Mode { - MaroonSim, - VrConnector, - } - - private GenericRobotMain robot; - private SimulatorInterface simulator; - - @SuppressWarnings("StaticAssignmentInConstructor") - public RobotMain(final Mode mode) { - try { - // TODO: update this to come from deploy directory? - ConfigFileReader.instance = new ConfigFileReader("simConfig.txt"); - RobotProvider.instance = new SimulationRobotProvider(); - - robot = new GenericRobotMain(); - - robot.robotInit(); - } catch (Exception exc) { - exc.printStackTrace(); - LoggerExceptionUtils.logException(exc); - } - - switch (mode) { - case MaroonSim -> { - simulator = new Simulator(); - } - case VrConnector -> { - ProgramInterface.robotMode = ProgramInterface.RobotMode.DISABLED; - try { - simulator = new VrConnector(); - } catch (IOException ex) { - throw new RuntimeException( - "Error initializing communication with 3d Simulator", ex); - } - } - } - - simulator.setResetHandler(() -> robot.resetAutonomousMode("simulation reset")); - } - - public void run() { - try { - ProgramInterface.RobotMode prevRobotMode = null; - while (true) { - simulator.prepareStep(); - - switch (ProgramInterface.robotMode) { - case DISABLED -> { - if (prevRobotMode != ProgramInterface.RobotMode.DISABLED) { - robot.disabledInit(); - prevRobotMode = ProgramInterface.RobotMode.DISABLED; - } - robot.disabledPeriodic(); - } - case AUTON -> { - if (prevRobotMode != ProgramInterface.RobotMode.AUTON) { - robot.autonomousInit(); - prevRobotMode = ProgramInterface.RobotMode.AUTON; - } - robot.autonomousPeriodic(); - } - case TELEOP -> { - if (prevRobotMode != ProgramInterface.RobotMode.TELEOP) { - robot.teleopInit(); - prevRobotMode = ProgramInterface.RobotMode.TELEOP; - } - robot.teleopPeriodic(); - } - } + public static void main(final String[] args) throws IOException { + var process = new ProcessBuilder().command("./gradlew", "simulateJava").start(); + while (true) { + try { + System.exit(process.waitFor()); + } catch (InterruptedException e) { } - } catch (Exception exc) { - exc.printStackTrace(); - LoggerExceptionUtils.logException(exc); - } - } - - public static void main(final String[] args) { - if (args.length != 1) { - System.err.println("Needs -maroon_sim or -vr_connector"); - System.exit(1); - } - Mode mode; - switch (args[0]) { - case "-maroon_sim": - mode = Mode.MaroonSim; - break; - case "-vr_connector": - mode = Mode.VrConnector; - break; - default: - System.err.println("Needs -maroon_sim or -vr_connector"); - System.exit(1); - return; } - new RobotMain(mode).run(); } } From 5be63cd91b673572d022f2ee5c2cdcdb5ad48bc8 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Mon, 15 Sep 2025 16:03:30 +0000 Subject: [PATCH 03/11] Improve vscode tasks --- .gitignore | 2 +- .vscode/launch.json | 15 +--- .vscode/tasks.json | 52 +++++++++++- deploy_sim.py | 79 +++++++++++++++++++ deploy_sim.sh | 29 ------- .../com/team766/hal/simulator/RobotMain.java | 17 ---- .../com/team766/hal/wpilib/RobotMain.java | 2 + 7 files changed, 135 insertions(+), 61 deletions(-) create mode 100755 deploy_sim.py delete mode 100755 deploy_sim.sh delete mode 100755 src/main/java/com/team766/hal/simulator/RobotMain.java diff --git a/.gitignore b/.gitignore index 7437c1421..fd268dd3c 100644 --- a/.gitignore +++ b/.gitignore @@ -189,4 +189,4 @@ compile_commands.json .factorypath # Don't commit the cache of downloaded sim files -/buildSim/ \ No newline at end of file +/build-sim/ \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json index 0875db2b6..7e1a4215c 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -23,18 +23,11 @@ "desktop": false, }, { - "type": "java", - "name": "Start 3d simulation mode", - "request": "launch", - "mainClass": "com.team766.hal.simulator.RobotMain", - "args": ["-vr_connector"] - }, - { - "type": "java", - "name": "Start Maroon simulation", + "type": "wpilib", + "name": "Run simulation", "request": "launch", - "mainClass": "com.team766.hal.simulator.RobotMain", - "args": ["-maroon_sim"] + "desktop": true, + "preLaunchTask": "Run Simulator" } ] } diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 2dfdc5301..de4b09e29 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -3,20 +3,66 @@ // for the documentation about the tasks.json format "version": "2.0.0", "tasks": [ + { + "label": "Run Simulator", + "type": "shell", + "command": "./deploy_sim.py", + "hide": true, + "showOutput": "always", + "isBackground": true, + "problemMatcher": { + "pattern": { + "regexp": "Simulation doesn't report errors so we don't need a pattern" + }, + "background": { + "activeOnStart": true, + "beginsPattern": "Simulation never restarts so we don't need a beginPattern", + "endsPattern": ".*Waiting for robot code to start.*" + } + }, + "presentation": { + "group": "robotSim", + "echo": true, + "reveal": "always", + "panel": "dedicated", + "showReuseMessage": false, + "clear": true, + "close": true + } + }, + { + "label": "Cleanup old robot code instances", + "type": "shell", + "command": "pkill --signal TERM --full simulateJava || true", + "hide": true, + "problemMatcher": [], + "presentation": { + "group": "robotSim", + "echo": true, + "showReuseMessage": false, + "clear": true, + "close": true + }, + }, { "label": "Deploy Sim", "type": "shell", - "command": "./deploy_sim.sh", + "command": "./gradlew simulateJava", "problemMatcher": [], "showOutput": "always", "presentation": { + "group": "robotSim", "echo": true, "reveal": "always", - "focus": true, "panel": "dedicated", "showReuseMessage": false, "clear": true - } + }, + "dependsOrder": "sequence", + "dependsOn": [ + "Cleanup old robot code instances", + "Run Simulator" + ] }, { "label": "Gradle Build", diff --git a/deploy_sim.py b/deploy_sim.py new file mode 100755 index 000000000..42422a143 --- /dev/null +++ b/deploy_sim.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 + +import os +import shutil +import socket +import subprocess +import time + +def main(argv=None): + os.chdir(os.path.dirname(os.path.abspath(__file__))) + project_base = os.getcwd() + + os.makedirs("build-sim", exist_ok=True) + os.chdir("build-sim") + + sim_package = "sim.tar.gz" + try: + t1 = os.path.getmtime(sim_package) + except OSError: + t1 = None + subprocess.check_call([ + "wget", + "-N", + f"https://github.com/Team766/2020Sim/releases/latest/download/{sim_package}", + ]) + t2 = os.path.getmtime(sim_package) + + extracted_dir = "files" + if t1 != t2 or not os.path.isdir(extracted_dir): + shutil.rmtree(extracted_dir, ignore_errors=True) + os.makedirs(extracted_dir, exist_ok=True) + subprocess.check_call([ + "tar", + f"--directory={extracted_dir}", + "-xf", + sim_package, + ]) + + # This is a backwards compatibility shim for existing 3d simulator packages. + os.makedirs("com/team766/hal/simulator", exist_ok=True) + with open("com/team766/hal/simulator/RobotMain.java", "w") as fd: + fd.write(r""" + package com.team766.hal.simulator; + public class RobotMain { + public static void main(final String[] args) { + while (true) { + try { + Thread.sleep(10000); + } catch (InterruptedException e) { + } + } + } + } + """) + subprocess.check_call(["javac", "com/team766/hal/simulator/RobotMain.java"]) + + os.chdir(extracted_dir) + + print("Waiting for robot code to start") + connected = False + iter = 0 + while not connected: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + try: + s.connect(("localhost", 5800)) + connected = True + except ConnectionRefusedError: + iter += 1 + print("Waiting for robot code to start" + "." * iter, end="\r") + time.sleep(5) + print() + + os.execl("./run.sh", "./run.sh", project_base, f"{project_base}/build-sim") + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + pass \ No newline at end of file diff --git a/deploy_sim.sh b/deploy_sim.sh deleted file mode 100755 index 2a7240bab..000000000 --- a/deploy_sim.sh +++ /dev/null @@ -1,29 +0,0 @@ -#!/bin/bash - -set -euo pipefail - -builtin cd "$(dirname -- "${BASH_SOURCE[0]}")" -project_base="$(pwd)" - -./gradlew jar - -jar_file=( $(readlink -f build/libs/*.jar) ) -[ "${#jar_file[@]}" -eq 1 ] || (echo "Output jar file could not be determined"; exit 1) - -mkdir -p buildSim -cd buildSim - -sim_package="sim.tar.gz" -t1="$(stat -c %y "$sim_package" || true)" -wget -N "https://github.com/Team766/2020Sim/releases/latest/download/$sim_package" || [ -f "$sim_package" ] || exit 1 -t2="$(stat -c %y "$sim_package")" - -extracted_dir="files" -if [ "$t1" != "$t2" -o ! -d "$extracted_dir" ]; then - rm -rf "$extracted_dir" - mkdir -p "$extracted_dir" - tar --directory="$extracted_dir" -xf "$sim_package" -fi -cd "$extracted_dir" - -exec ./run.sh "$project_base" "$jar_file" \ No newline at end of file diff --git a/src/main/java/com/team766/hal/simulator/RobotMain.java b/src/main/java/com/team766/hal/simulator/RobotMain.java deleted file mode 100755 index 2072f4764..000000000 --- a/src/main/java/com/team766/hal/simulator/RobotMain.java +++ /dev/null @@ -1,17 +0,0 @@ -package com.team766.hal.simulator; - -import java.io.IOException; - -// This is a backwards compatibility shim for existing 3d simulator packages. -// New applications should run `./gradlew simulateJava` directly. -public class RobotMain { - public static void main(final String[] args) throws IOException { - var process = new ProcessBuilder().command("./gradlew", "simulateJava").start(); - while (true) { - try { - System.exit(process.waitFor()); - } catch (InterruptedException e) { - } - } - } -} diff --git a/src/main/java/com/team766/hal/wpilib/RobotMain.java b/src/main/java/com/team766/hal/wpilib/RobotMain.java index b9c8a50de..c98a7745f 100755 --- a/src/main/java/com/team766/hal/wpilib/RobotMain.java +++ b/src/main/java/com/team766/hal/wpilib/RobotMain.java @@ -193,6 +193,8 @@ enum SimulationMode { } } simulator.setResetHandler(() -> robot.resetAutonomousMode("simulation reset")); + // Do an initial step here to flush any needed state initialization. + simulator.prepareStep(); } catch (Exception exc) { exc.printStackTrace(); LoggerExceptionUtils.logException(exc); From c2a2f5b79558154cb5b7786fe5efd51896a2d51d Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Tue, 16 Sep 2025 15:08:52 +0000 Subject: [PATCH 04/11] reactivate simulator task after starting robot code --- .vscode/tasks.json | 3 +-- deploy_sim.py | 67 ++++++++++++++++++++++++++++++++++++---------- 2 files changed, 54 insertions(+), 16 deletions(-) diff --git a/.vscode/tasks.json b/.vscode/tasks.json index de4b09e29..784ca2737 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -16,7 +16,7 @@ }, "background": { "activeOnStart": true, - "beginsPattern": "Simulation never restarts so we don't need a beginPattern", + "beginsPattern": ".*Robot code started.*", "endsPattern": ".*Waiting for robot code to start.*" } }, @@ -27,7 +27,6 @@ "panel": "dedicated", "showReuseMessage": false, "clear": true, - "close": true } }, { diff --git a/deploy_sim.py b/deploy_sim.py index 42422a143..00382cd0b 100755 --- a/deploy_sim.py +++ b/deploy_sim.py @@ -1,11 +1,58 @@ #!/usr/bin/env python3 +import ctypes +import ctypes.util import os import shutil +import signal import socket import subprocess +import threading import time +# Constant taken from http://linux.die.net/include/linux/prctl.h +PR_SET_PDEATHSIG = 1 + +class PrCtlError(Exception): + pass + +def set_parent_exit_signal(): + """ + Return a function to be run in a child process which will trigger SIGNAME + to be sent when the parent process dies + """ + # http://linux.die.net/man/2/prctl + libc = ctypes.CDLL(ctypes.util.find_library("c"), use_errno=True) + if libc.prctl(PR_SET_PDEATHSIG, signal.SIGABRT) != 0: + errno = ctypes.get_errno() + raise OSError(errno, f"SET_PDEATHSIG prctl failed: {os.strerror(errno)}") + +class TerminatingPopen(subprocess.Popen): + def __exit__(self, exc_type, value, traceback) -> None: + self.terminate() + return super().__exit__(exc_type, value, traceback) + +connected = None + +def code_monitor_thread(): + global connected + iter = 0 + while True: + try: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + s.connect(("localhost", 5800)) + if connected != True: + print("Robot code started") + connected = True + iter = 0 + except ConnectionRefusedError: + if connected != False: + print("Waiting for robot code to start") + connected = False + iter += 1 + print("." * (iter % 6) + " ", end="\r") + time.sleep(1) + def main(argv=None): os.chdir(os.path.dirname(os.path.abspath(__file__))) project_base = os.getcwd() @@ -56,21 +103,13 @@ def main(argv=None): os.chdir(extracted_dir) - print("Waiting for robot code to start") - connected = False - iter = 0 + threading.Thread(target=code_monitor_thread, daemon=True).start() + while not connected: - with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: - try: - s.connect(("localhost", 5800)) - connected = True - except ConnectionRefusedError: - iter += 1 - print("Waiting for robot code to start" + "." * iter, end="\r") - time.sleep(5) - print() - - os.execl("./run.sh", "./run.sh", project_base, f"{project_base}/build-sim") + time.sleep(1) + + with TerminatingPopen(["./run.sh", project_base, f"{project_base}/build-sim"]) as p: + p.wait() if __name__ == "__main__": try: From 5d46eae53828e41c3f521761da37fc46daf8ce66 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Tue, 16 Sep 2025 16:17:32 +0000 Subject: [PATCH 05/11] comments --- .vscode/tasks.json | 14 ++++++++++++++ deploy_sim.py | 32 +++++++++++++++++++++++++++++--- 2 files changed, 43 insertions(+), 3 deletions(-) diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 784ca2737..b005dfdf2 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -3,12 +3,20 @@ // for the documentation about the tasks.json format "version": "2.0.0", "tasks": [ + // The "Run Simulator" task starts the simulator to run in parallel with the robot code. { "label": "Run Simulator", "type": "shell", "command": "./deploy_sim.py", "hide": true, "showOutput": "always", + // This task is a dependency of the Deploy Sim task which runs the robot code, and so + // it needs to finish before the robot code will be allowed to start running. However, + // we want the simulator to run in parallel with the robot code, so we set it as a + // background task, which allows a task to signal when it has finished (with console + // output that matches the endsPattern of the problemMatcher) and then continue running + // in the background. We then reactivate the task after the robot code starts running + // (using the beginsPattern). "isBackground": true, "problemMatcher": { "pattern": { @@ -29,6 +37,10 @@ "clear": true, } }, + // The "Cleanup old robot code instances" task is used to terminate any existing instances + // of the robot code before starting a new one. + // Codespaces can orphan task processes if the user refreshes the browser tab or + // disconnects, so we need to manually clean them up. { "label": "Cleanup old robot code instances", "type": "shell", @@ -59,6 +71,8 @@ }, "dependsOrder": "sequence", "dependsOn": [ + // Before starting the robot code, make sure that any previous instances are + // stopped, and that the simulator is started. "Cleanup old robot code instances", "Run Simulator" ] diff --git a/deploy_sim.py b/deploy_sim.py index 00382cd0b..7f4138dfa 100755 --- a/deploy_sim.py +++ b/deploy_sim.py @@ -18,12 +18,12 @@ class PrCtlError(Exception): def set_parent_exit_signal(): """ - Return a function to be run in a child process which will trigger SIGNAME - to be sent when the parent process dies + This uses a Linux-specific feature to configure the child process to receive a SIGTERM signal + when the parent process (this script) dies. This ensures we don't leave any orphaned processes. """ # http://linux.die.net/man/2/prctl libc = ctypes.CDLL(ctypes.util.find_library("c"), use_errno=True) - if libc.prctl(PR_SET_PDEATHSIG, signal.SIGABRT) != 0: + if libc.prctl(PR_SET_PDEATHSIG, signal.SIGTERM) != 0: errno = ctypes.get_errno() raise OSError(errno, f"SET_PDEATHSIG prctl failed: {os.strerror(errno)}") @@ -35,6 +35,11 @@ def __exit__(self, exc_type, value, traceback) -> None: connected = None def code_monitor_thread(): + """ + This is run in a daemon thread to monitor and report the state of the robot code to VS Code. + It determines whether the robot code is running by polling whether it can connect to the HTTP + server on port 5800 that's built into the robot code. + """ global connected iter = 0 while True: @@ -42,24 +47,35 @@ def code_monitor_thread(): with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: s.connect(("localhost", 5800)) if connected != True: + # If you change this print, also update the regex in .vscode/tasks.json print("Robot code started") connected = True iter = 0 except ConnectionRefusedError: if connected != False: + # If you change this print, also update the regex in .vscode/tasks.json print("Waiting for robot code to start") connected = False + # Display a simple animation to show signs of life while we're waiting for the + # robot code to start. iter += 1 print("." * (iter % 6) + " ", end="\r") time.sleep(1) def main(argv=None): + # Handle the case where somebody tried to run this script from somewhere besides the root + # directory of the repo. os.chdir(os.path.dirname(os.path.abspath(__file__))) project_base = os.getcwd() + # Create a directory in which to keep all of the simulator's files. os.makedirs("build-sim", exist_ok=True) os.chdir("build-sim") + # Download the simulator package from github. + # The -N flag to wget skips the download if we already have the most recent version (as + # determined by comparing the modified timestamp of the file to the Last-Modified header + # returned by the server). sim_package = "sim.tar.gz" try: t1 = os.path.getmtime(sim_package) @@ -72,6 +88,7 @@ def main(argv=None): ]) t2 = os.path.getmtime(sim_package) + # Extract the simulator package (unless we've already extracted the most recent version). extracted_dir = "files" if t1 != t2 or not os.path.isdir(extracted_dir): shutil.rmtree(extracted_dir, ignore_errors=True) @@ -84,6 +101,9 @@ def main(argv=None): ]) # This is a backwards compatibility shim for existing 3d simulator packages. + # Previously, the run script of the simulator was also responsible for running the robot code, + # so give it some placeholder Java code to run. The simulator has a hard-coded Java class that + # it expects to run. os.makedirs("com/team766/hal/simulator", exist_ok=True) with open("com/team766/hal/simulator/RobotMain.java", "w") as fd: fd.write(r""" @@ -103,11 +123,17 @@ def main(argv=None): os.chdir(extracted_dir) + # Wait until the robot code has started before starting the simulator. + # This is because the simulator's run script prints the link to the viewer webpage, but we don't + # want the user opening that until the robot code has actually started running (otherwise, the + # right-side pane that tries to display data from the HTTP server built into the robot code will + # fail to load). threading.Thread(target=code_monitor_thread, daemon=True).start() while not connected: time.sleep(1) + # Invoke the simulator package's entrypoint. with TerminatingPopen(["./run.sh", project_base, f"{project_base}/build-sim"]) as p: p.wait() From a023e06ee6da025f91403bcf4cb5758c1f8dbc13 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Fri, 19 Sep 2025 08:33:48 +0000 Subject: [PATCH 06/11] run the simulator in a screen session --- deploy_sim.py | 91 ++++++++++++++++----------------------------------- 1 file changed, 29 insertions(+), 62 deletions(-) diff --git a/deploy_sim.py b/deploy_sim.py index 7f4138dfa..77606b05c 100755 --- a/deploy_sim.py +++ b/deploy_sim.py @@ -1,73 +1,21 @@ #!/usr/bin/env python3 -import ctypes -import ctypes.util import os import shutil -import signal import socket import subprocess -import threading import time -# Constant taken from http://linux.die.net/include/linux/prctl.h -PR_SET_PDEATHSIG = 1 - -class PrCtlError(Exception): - pass - -def set_parent_exit_signal(): - """ - This uses a Linux-specific feature to configure the child process to receive a SIGTERM signal - when the parent process (this script) dies. This ensures we don't leave any orphaned processes. - """ - # http://linux.die.net/man/2/prctl - libc = ctypes.CDLL(ctypes.util.find_library("c"), use_errno=True) - if libc.prctl(PR_SET_PDEATHSIG, signal.SIGTERM) != 0: - errno = ctypes.get_errno() - raise OSError(errno, f"SET_PDEATHSIG prctl failed: {os.strerror(errno)}") - -class TerminatingPopen(subprocess.Popen): - def __exit__(self, exc_type, value, traceback) -> None: - self.terminate() - return super().__exit__(exc_type, value, traceback) - -connected = None - -def code_monitor_thread(): - """ - This is run in a daemon thread to monitor and report the state of the robot code to VS Code. - It determines whether the robot code is running by polling whether it can connect to the HTTP - server on port 5800 that's built into the robot code. - """ - global connected - iter = 0 - while True: - try: - with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: - s.connect(("localhost", 5800)) - if connected != True: - # If you change this print, also update the regex in .vscode/tasks.json - print("Robot code started") - connected = True - iter = 0 - except ConnectionRefusedError: - if connected != False: - # If you change this print, also update the regex in .vscode/tasks.json - print("Waiting for robot code to start") - connected = False - # Display a simple animation to show signs of life while we're waiting for the - # robot code to start. - iter += 1 - print("." * (iter % 6) + " ", end="\r") - time.sleep(1) - def main(argv=None): # Handle the case where somebody tried to run this script from somewhere besides the root # directory of the repo. os.chdir(os.path.dirname(os.path.abspath(__file__))) project_base = os.getcwd() + # Install dependencies + subprocess.check_call(["sudo", "apt", "update"]) + subprocess.check_call(["sudo", "apt", "install", "-y", "screen"]) + # Create a directory in which to keep all of the simulator's files. os.makedirs("build-sim", exist_ok=True) os.chdir("build-sim") @@ -123,19 +71,38 @@ def main(argv=None): os.chdir(extracted_dir) + # If you change this print, also update the regex in .vscode/tasks.json + print("Waiting for robot code to start") + # Wait until the robot code has started before starting the simulator. + # It determines whether the robot code is running by polling whether it can connect to the HTTP + # server on port 5800 that's built into the robot code. # This is because the simulator's run script prints the link to the viewer webpage, but we don't # want the user opening that until the robot code has actually started running (otherwise, the # right-side pane that tries to display data from the HTTP server built into the robot code will # fail to load). - threading.Thread(target=code_monitor_thread, daemon=True).start() - - while not connected: - time.sleep(1) + iter = 0 + while True: + try: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + s.connect(("localhost", 5800)) + break + except ConnectionRefusedError: + # Display a simple animation to show signs of life while we're waiting for the + # robot code to start. + iter += 1 + print("." * (iter % 6) + " ", end="\r") + time.sleep(1) # Invoke the simulator package's entrypoint. - with TerminatingPopen(["./run.sh", project_base, f"{project_base}/build-sim"]) as p: - p.wait() + # NOTE(2025-09-19): There appears to be some issue in Codespaces right now that causes + # the Run Simulator task to randomly restart (sometimes accompanied by the message + # "Extension Host Process exited with code: null, signal: SIGTERM"). Thus, we run the simulator + # in a GNU Screen session so that if the task restarts, we can just reconnect to the existing + # instance of the simulator without any disruption to the user. + os.execlp( + "screen", "screen", "-D", "-R", "-S", "simulator", + "./run.sh", project_base, f"{project_base}/build-sim") if __name__ == "__main__": try: From 92ef0b0ebde58d462894ae2422af5e548c5c6d60 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Mon, 29 Sep 2025 04:47:16 -0700 Subject: [PATCH 07/11] Remove sim entrypoint shim --- deploy_sim.py | 23 +---------------------- 1 file changed, 1 insertion(+), 22 deletions(-) diff --git a/deploy_sim.py b/deploy_sim.py index 77606b05c..96b3454b8 100755 --- a/deploy_sim.py +++ b/deploy_sim.py @@ -47,27 +47,6 @@ def main(argv=None): "-xf", sim_package, ]) - - # This is a backwards compatibility shim for existing 3d simulator packages. - # Previously, the run script of the simulator was also responsible for running the robot code, - # so give it some placeholder Java code to run. The simulator has a hard-coded Java class that - # it expects to run. - os.makedirs("com/team766/hal/simulator", exist_ok=True) - with open("com/team766/hal/simulator/RobotMain.java", "w") as fd: - fd.write(r""" - package com.team766.hal.simulator; - public class RobotMain { - public static void main(final String[] args) { - while (true) { - try { - Thread.sleep(10000); - } catch (InterruptedException e) { - } - } - } - } - """) - subprocess.check_call(["javac", "com/team766/hal/simulator/RobotMain.java"]) os.chdir(extracted_dir) @@ -102,7 +81,7 @@ def main(argv=None): # instance of the simulator without any disruption to the user. os.execlp( "screen", "screen", "-D", "-R", "-S", "simulator", - "./run.sh", project_base, f"{project_base}/build-sim") + "./run.sh", project_base) if __name__ == "__main__": try: From 24a30dcc4ccbf92f760c144441ec736e8679952f Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Mon, 29 Sep 2025 04:47:43 -0700 Subject: [PATCH 08/11] Install `screen` in the container setup --- .devcontainer/Dockerfile | 7 +++++++ .devcontainer/devcontainer.json | 2 +- deploy_sim.py | 4 ---- 3 files changed, 8 insertions(+), 5 deletions(-) create mode 100644 .devcontainer/Dockerfile diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 000000000..9d4ed3626 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,7 @@ +FROM mcr.microsoft.com/devcontainers/java:1-17-bookworm + +RUN apt update && \ + export DEBIAN_FRONTEND=noninteractive && \ + apt install -y --no-install-recommends \ + screen \ + && rm -rf /var/lib/apt/lists/* \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 343e12157..25562d471 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -2,7 +2,7 @@ // README at: https://github.com/devcontainers/templates/tree/main/src/java { "name": "Java", - "image": "mcr.microsoft.com/devcontainers/java:1-17-bookworm", + "build": { "dockerfile": "Dockerfile" }, "features": { "ghcr.io/devcontainers/features/java:1": { diff --git a/deploy_sim.py b/deploy_sim.py index 96b3454b8..596894144 100755 --- a/deploy_sim.py +++ b/deploy_sim.py @@ -12,10 +12,6 @@ def main(argv=None): os.chdir(os.path.dirname(os.path.abspath(__file__))) project_base = os.getcwd() - # Install dependencies - subprocess.check_call(["sudo", "apt", "update"]) - subprocess.check_call(["sudo", "apt", "install", "-y", "screen"]) - # Create a directory in which to keep all of the simulator's files. os.makedirs("build-sim", exist_ok=True) os.chdir("build-sim") From 3891d675618e769b9e6a629972d694ad4c4d8285 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Mon, 29 Sep 2025 04:51:26 -0700 Subject: [PATCH 09/11] remove condition on isReal --- .../com/team766/hal/wpilib/RobotMain.java | 45 ++++++++++--------- src/main/java/com/team766/logging/Logger.java | 34 +++++++------- 2 files changed, 38 insertions(+), 41 deletions(-) diff --git a/src/main/java/com/team766/hal/wpilib/RobotMain.java b/src/main/java/com/team766/hal/wpilib/RobotMain.java index c98a7745f..45d990807 100755 --- a/src/main/java/com/team766/hal/wpilib/RobotMain.java +++ b/src/main/java/com/team766/hal/wpilib/RobotMain.java @@ -95,42 +95,41 @@ private static String checkForAndReturnPathToConfigFile(final String file) { @Override public void robotInit() { try { - boolean configFromUSB = true; - String filename = checkForAndReturnPathToConfigFile(USB_CONFIG_FILE); - - if (filename == null) { - filename = INTERNAL_CONFIG_FILE; - configFromUSB = false; - } - if (isSimulation()) { ConfigFileReader.instance = new ConfigFileReader("simConfig.txt"); // TODO: Use WPILib's simulation interfaces and switch this to WPIRobotProvider RobotProvider.instance = new SimulationRobotProvider(); } else { + boolean configFromUSB = true; + String filename = checkForAndReturnPathToConfigFile(USB_CONFIG_FILE); + if (filename == null) { + filename = INTERNAL_CONFIG_FILE; + configFromUSB = false; + } ConfigFileReader.instance = new ConfigFileReader(filename, configFromUSB ? INTERNAL_CONFIG_FILE : null); RobotProvider.instance = new WPIRobotProvider(); } - robot = new GenericRobotMain(); - DriverStation.startDataLog(DataLogManager.getLog()); + var configLogDir = com.team766.logging.Logger.getLogDirFromConfig(); + if (configLogDir.hasValue()) { + new File(configLogDir.get()).mkdirs(); + } + DataLogManager.start(configLogDir.valueOr("" /* use DataLogManager's default dir */)); + com.team766.logging.Logger.init(DataLogManager.getLogDir()); - if (isReal()) { - // enable dual-logging - com.team766.logging.Logger.enableLoggingToDataLog(true); + DriverStation.startDataLog(DataLogManager.getLog()); - // set up AdvantageKit logging - DataLogManager.log("Initializing logging."); - Logger.addDataReceiver(new WPILOGWriter("/U/logs")); // Log to sdcard - if (!DriverStation.isFMSAttached()) { - Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - } - new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging + // enable dual-logging + com.team766.logging.Logger.enableLoggingToDataLog(true); - } else { - // TODO: add support for simulation logging/replay + // set up AdvantageKit logging + DataLogManager.log("Initializing logging."); + Logger.addDataReceiver(new WPILOGWriter(DataLogManager.getLogDir())); // Log to sdcard + if (!DriverStation.isFMSAttached()) { + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables } + new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); Logger.recordMetadata("GitDirty", BuildConstants.DIRTY != 0 ? "Yes" : "No"); @@ -138,6 +137,8 @@ public void robotInit() { Logger.start(); + robot = new GenericRobotMain(); + robot.robotInit(); } catch (Exception e) { e.printStackTrace(); diff --git a/src/main/java/com/team766/logging/Logger.java b/src/main/java/com/team766/logging/Logger.java index b0b52875e..d130fa0dc 100644 --- a/src/main/java/com/team766/logging/Logger.java +++ b/src/main/java/com/team766/logging/Logger.java @@ -3,6 +3,7 @@ import com.google.errorprone.annotations.FormatMethod; import com.team766.config.ConfigFileReader; import com.team766.library.CircularBuffer; +import com.team766.library.ValueProvider; import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.wpilibj.DataLogManager; import java.io.File; @@ -54,30 +55,25 @@ public void uncaughtException(final Thread t, final Throwable e) { for (Category category : Category.values()) { m_loggers.put(category, new Logger(category)); } + + Thread.setDefaultUncaughtExceptionHandler(new LogUncaughtException()); + } + + public static ValueProvider getLogDirFromConfig() { + return ConfigFileReader.getInstance().getString(LOG_FILE_PATH_KEY); + } + + public static void init(String logDir) { try { - ConfigFileReader config_file = ConfigFileReader.getInstance(); - if (config_file != null && config_file.containsKey(LOG_FILE_PATH_KEY)) { - logFilePathBase = config_file.getString(LOG_FILE_PATH_KEY).get(); - new File(logFilePathBase).mkdirs(); - final String timestamp = - new SimpleDateFormat("yyyyMMdd'T'HHmmss").format(new Date()); - final String logFilePath = new File(logFilePathBase, timestamp).getAbsolutePath(); - m_logWriter = new LogWriter(logFilePath); - get(Category.CONFIGURATION).logRaw(Severity.INFO, "Logging to " + logFilePath); - } else { - get(Category.CONFIGURATION) - .logRaw( - Severity.ERROR, - "Config file does not specify " - + LOG_FILE_PATH_KEY - + ". Logs will only be in-memory and will be lost when the robot is turned off."); - } + logFilePathBase = logDir; + final String timestamp = new SimpleDateFormat("yyyyMMdd'T'HHmmss").format(new Date()); + final String logFilePath = new File(logFilePathBase, timestamp).getAbsolutePath(); + m_logWriter = new LogWriter(logFilePath); + get(Category.CONFIGURATION).logRaw(Severity.INFO, "Logging to " + logFilePath); } catch (Exception e) { e.printStackTrace(); LoggerExceptionUtils.logException(e); } - - Thread.setDefaultUncaughtExceptionHandler(new LogUncaughtException()); } public static void enableLoggingToDataLog(boolean enabled) { From e7934717dfddb0217ea826f0a5fbc54eef4dc7d4 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Sat, 4 Oct 2025 16:50:50 -0700 Subject: [PATCH 10/11] Make simulation mode work better when simulator isn't present --- src/main/java/com/team766/hal/simulator/VrConnector.java | 3 ++- src/main/java/com/team766/hal/wpilib/RobotMain.java | 5 +++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team766/hal/simulator/VrConnector.java b/src/main/java/com/team766/hal/simulator/VrConnector.java index 0962d0bb5..7296fb61c 100644 --- a/src/main/java/com/team766/hal/simulator/VrConnector.java +++ b/src/main/java/com/team766/hal/simulator/VrConnector.java @@ -229,7 +229,7 @@ private boolean process() throws IOException { } selector.selectedKeys().clear(); - selector.select(); + selector.select(3000); boolean newData = false; for (SelectionKey key : selector.selectedKeys()) { if (!key.isValid()) { @@ -373,6 +373,7 @@ public double prepareStep() { if (ProgramInterface.simulationTime == 0) { // Wait for a connection to the simulator before starting to run the robot code. startTime = System.currentTimeMillis(); + System.out.println("Waiting for 3d simulator to connect"); continue; } if (resetCounter != lastResetCounter) { diff --git a/src/main/java/com/team766/hal/wpilib/RobotMain.java b/src/main/java/com/team766/hal/wpilib/RobotMain.java index 45d990807..88465905f 100755 --- a/src/main/java/com/team766/hal/wpilib/RobotMain.java +++ b/src/main/java/com/team766/hal/wpilib/RobotMain.java @@ -170,12 +170,17 @@ public void autonomousInit() { public void simulationInit() { try { enum SimulationMode { + None, MaroonSim, VrConnector, } final var simulationMode = ConfigFileReader.getInstance().getEnum(SimulationMode.class, "simulationMode"); switch (simulationMode.get()) { + case None -> { + simulator = null; + return; + } case MaroonSim -> { com.team766.logging.Logger.get(Category.FRAMEWORK) .logRaw(Severity.INFO, "Running Maroon simulator"); From c24d9dd5274e0fd387d5f1d6b2d9e3984a69a058 Mon Sep 17 00:00:00 2001 From: Chris Padwick Date: Sun, 12 Oct 2025 16:47:38 -0700 Subject: [PATCH 11/11] Add AprilTag simulation support. --- CLAUDE.md | 141 +++++++++ docs/AprilTagSimulation.md | 273 ++++++++++++++++++ simConfig.txt | 1 + .../com/team766/hal/wpilib/RobotMain.java | 18 ++ .../team766/simulator/AprilTagSimulator.java | 177 ++++++++++++ 5 files changed, 610 insertions(+) create mode 100644 CLAUDE.md create mode 100644 docs/AprilTagSimulation.md create mode 100644 src/main/java/com/team766/simulator/AprilTagSimulator.java diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 000000000..0de33d22f --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,141 @@ +# CLAUDE.md + +This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. + +## Project Overview + +This is Team 766's FRC (FIRST Robotics Competition) 2025 robot code, written in Java using WPILib. The codebase uses a custom framework built around a Mechanism/Procedure architecture with resource reservation, plus an integrated physics simulator for testing without hardware. + +## Build and Development Commands + +### Building and Testing +- `./gradlew build` - Compile code and run all tests +- `./gradlew test` - Run tests only +- `./gradlew spotlessApply` - Auto-format code (Java, Gradle, XML) using Spotless +- `./gradlew spotlessCheck` - Check code formatting without applying changes + +### Deployment +- `./gradlew deploy` - Deploy to the RoboRIO (robot must be connected) +- Config files are deployed from `src/main/deploy/configs/` to `/home/lvuser/deploy/configs` on the robot + +### Simulation +- Run the "Simulate Robot Code" task in VS Code to start robot code in simulation mode +- `./deploy_sim.py` - Start the external physics simulator (waits for robot code on port 5800, then launches in a `screen` session) +- Simulation mode is configured via `simConfig.txt` (JSON format) +- The simulator provides a web-based 3D visualization of robot physics + +## Architecture + +### Framework Layer (`com.team766.framework`) +The custom framework provides the core abstractions: + +**Mechanism**: Base class for all robot subsystems. Mechanisms must be reserved before use via Procedures. Each Mechanism has a `run()` method called periodically and an `onMechanismIdle()` method called when no Procedure is using it. Mechanisms integrate with WPILib2's Command system via internal proxy subsystems. + +**Procedure**: Asynchronous tasks that can reserve Mechanisms. Procedures receive a `Context` object that provides cooperative-multitasking primitives like `waitFor()`, `waitForSeconds()`, etc. Procedures run as WPILib2 Commands under the hood. + +**Context**: Execution context for Procedures providing wait/yield operations. Procedures can be interrupted if another Procedure with overlapping Mechanism reservations is started. + +**Rule/RuleEngine**: Event-driven behavior system. Rules trigger Procedures based on conditions (e.g., button presses in operator interface). Used for teleoperated control. + +### Hardware Abstraction Layer (`com.team766.hal`) +Abstraction layer over WPILib hardware APIs: +- Multiple implementations: `wpilib` (real robot), `simulator` (integrated sim), `mock` (unit tests) +- `RobotProvider` provides factory methods for motors, sensors, etc. +- `MotorController` interface for motor control with position/velocity PID, current limiting, and sensor scaling +- Configuration loaded from JSON files at runtime (see Configuration section) + +### Robot Implementations (`com.team766.robot`) +Multiple robot configurations in separate packages: +- `reva` - 2024 robot with swerve drive, shoulder, intake, shooter, climber +- `reva_2025` - Active development for 2025 season +- `common` - Shared code including swerve drive implementation +- `example`, `gatorade`, etc. - Other robot configurations + +Each robot package contains: +- `Robot.java` - Implements `RobotConfigurator` to initialize mechanisms, operator interface, and autonomous modes +- `mechanisms/` - Mechanism implementations +- `procedures/` - Procedure implementations for autonomous and complex actions +- `OI.java` - RuleEngine implementation that binds controls to Procedures + +### Swerve Drive +Swerve drive is in `com.team766.robot.common.mechanisms.SwerveDrive`: +- Field-oriented control with gyro-based rotation compensation +- Independent control of translation and rotation +- Cross-wheels mode for resisting movement +- Odometry integration for position tracking +- Configuration via `SwerveConfig` class (wheel locations, CAN bus, current limits) +- Controlled via `controlFieldOriented(x, y, rotation)` or `drive(chassisSpeeds)` + +See `docs/SwerveDrive.md` for detailed implementation notes and bringup instructions. + +### Operator Interface +Controls are organized by role: +- **Driver** (`DriverOI`) - Two Thrustmaster T.16000M joysticks for robot movement +- **Box Operator** (`BoxOpOI`) - Xbox controller for mechanism control +- **Debug** (`DebugOI`) - Megalodon macro pad for pit testing individual mechanisms + +See `docs/OperatorInterface.md` for complete control mappings. + +## Configuration System + +Robot hardware configuration is stored in JSON files: +- Main config: `simConfig.txt` (JSON despite extension) at project root +- Additional configs can be in `src/main/deploy/configs/` +- Config is read via `com.team766.config.ConfigFileReader` +- Access config values using `ConfigValue` types in code +- Supports motor controllers, sensors, PID constants, physical parameters +- Specify which robot configuration to load via `"robotConfigurator"` field pointing to a class implementing `RobotConfigurator` + +## Logging and Telemetry + +Uses AdvantageKit for logging: +- `@AutoLog` annotation for automatic logging of mechanism state +- Logs written to `logs/` directory +- Web dashboard on port 5800 during simulation +- Replay logs using `./gradlew replayWatch` + +## Testing + +- JUnit 5 for unit tests +- ErrorProne static analysis (custom check: `DontDiscardProcedures` ensures Procedures are scheduled) +- AspectJ used to enforce that Procedures are not discarded without being scheduled +- Mock HAL implementation for hardware-independent testing + +## Common Development Patterns + +### Creating a New Mechanism +1. Extend `Mechanism` (or `MechanismWithStatus` for status publishing) +2. Add config entries for hardware in config file +3. Initialize hardware in constructor using `RobotProvider.instance.getXXX(configValue)` +4. Implement `run()` for periodic updates +5. Implement `onMechanismIdle()` to stop/safe state when not reserved +6. Add to `Robot.initializeMechanisms()` + +### Creating a New Procedure +1. Extend `Procedure` or `InstantProcedure` +2. Call `reserve(mechanism)` in constructor for any Mechanisms you'll use +3. Implement `run(Context context)` with your logic +4. Use `context.waitFor()`, `context.waitForSeconds()`, etc. for timing +5. Trigger from OI using `Rule.when(condition).then(procedureName, mechanism1, mechanism2, ...)` + +### Creating a New Autonomous Mode +1. Create a Procedure for the autonomous routine in `procedures/auton_routines/` +2. Add to `Robot.getAutonomousModes()` array +3. Use WPILib `PathPlanner` for path following with swerve drive + +## Simulation Mode + +The codebase includes an integrated physics simulator: +- Set `"simulationMode": "VrConnector"` in config to enable +- Simulator runs separately and connects via network sockets +- Simulates motors, sensors, pneumatics, and robot physics +- Web UI shows 3D robot visualization and controls +- Implementation in `com.team766.simulator` package + +## Project Structure Notes + +- Generated code (protobuf, build constants) in `build/generated/` +- Vendor dependencies in `vendordeps/` (AdvantageKit, CTRE, REV, etc.) +- Uses Gradle with GradleRIO plugin for FRC-specific build tasks +- DevContainer support for consistent development environment +- CI via GitHub Actions: builds and tests on every push/PR diff --git a/docs/AprilTagSimulation.md b/docs/AprilTagSimulation.md new file mode 100644 index 000000000..6aa9e207b --- /dev/null +++ b/docs/AprilTagSimulation.md @@ -0,0 +1,273 @@ +# AprilTag Simulation + +This document explains how to simulate AprilTag detections during robot simulation, mimicking the behavior of the Orin coprocessor that normally provides vision data. + +## Overview + +In the real robot: +- **Orin coprocessor** runs vision processing +- Detects AprilTags using cameras +- Publishes detections to **NetworkTables** as double arrays +- **RoboRIO** reads these via `GetOrinRawValue` class +- Data format: `[timestamp, tagId, x, y, z, timestamp, tagId, x, y, z, ...]` + +In simulation: +- **AprilTagSimulator** replaces the Orin +- Publishes fake AprilTag detections based on simulated robot position +- Uses the same NetworkTables interface +- Robot code works identically - it just reads from NetworkTables + +## Architecture + +``` +┌──────────────────────────────────────────────────────────────┐ +│ Robot Code (simulateJava) │ +│ │ +│ ┌─────────────────┐ ┌──────────────────────┐ │ +│ │ RobotMain │ │ Orin Mechanism │ │ +│ │ simulationInit()│ │ │ │ +│ │ │ │ GetOrinRawValue │ │ +│ │ Creates │ │ .getRawPoseData() │ │ +│ │ AprilTagSim ────┼──────┐ │ reads from NT │ │ +│ └─────────────────┘ │ └──────────────────────┘ │ +│ │ │ +│ ┌─────────────────┐ │ │ +│ │ simulationPeriodic() │ │ │ +│ │ calls: │ │ │ +│ │ aprilTagSim │ │ │ +│ │ .simulateAprilTags() │ │ +│ └─────────────────┘ │ │ +│ │ │ +│ ┌─────────────────────▼───────────────────────┐ │ +│ │ AprilTagSimulator │ │ +│ │ │ │ +│ │ 1. Read robot position from │ │ +│ │ ProgramInterface.robotPosition │ │ +│ │ │ │ +│ │ 2. Calculate which field tags are visible │ │ +│ │ (within detection range) │ │ +│ │ │ │ +│ │ 3. Transform tag positions from field │ │ +│ │ frame to robot-relative frame │ │ +│ │ │ │ +│ │ 4. Publish to NetworkTables: │ │ +│ │ /SmartDashboard/ │ │ +│ └──────────────────┬──────────────────────────┘ │ +│ │ │ +└─────────────────────┼────────────────────────────────────────┘ + │ + │ NetworkTables + │ (UDP port 5810) + │ + ↓ + [Robot code reads it back via GetOrinRawValue] +``` + +## Usage + +### Option 1: Integrated Mode (Recommended) + +The AprilTag simulator runs **inside** the robot code process. This is simpler and already integrated. + +1. **Start simulation**: + ```bash + ./gradlew simulateJava + ``` + +2. **Configuration** (in `RobotMain.simulationInit()`): + ```java + aprilTagSimulator = new AprilTagSimulator( + "apriltag_detections", // NetworkTables topic name + 5.0, // Detection range in meters + 20.0 // Publish rate in Hz + ); + ``` + +3. **Configure field tags** (in `AprilTagSimulator.initializeFieldTags()`): + ```java + // Add tags at known field positions + fieldTags.add(new FieldAprilTag(1, 0.0, 0.0, 0.5)); + fieldTags.add(new FieldAprilTag(2, 5.0, 0.0, 0.5)); + // ... add more based on your field layout + ``` + +4. **Update your Orin mechanism** to use the correct topic: + ```java + // In your robot's Orin mechanism or wherever you create GetOrinRawValue: + GetOrinRawValue orinReader = new GetOrinRawValue("apriltag_detections", 0.1); + ``` + +### Option 2: Standalone Process + +Run the AprilTag simulator as a **separate Java process**. This better mimics the real Orin architecture. + +1. **Add to `build.gradle`**: + ```gradle + task runAprilTagSim(type: JavaExec) { + mainClass = "com.team766.simulator.StandaloneAprilTagSimulator" + classpath = sourceSets.main.runtimeClasspath + args = ["localhost", "apriltag_detections", "5.0", "20.0"] + // args: [ntServer, topicName, rangeMeters, rateHz] + } + ``` + +2. **Start robot code** (in one terminal): + ```bash + ./gradlew simulateJava + ``` + +3. **Start AprilTag simulator** (in another terminal): + ```bash + ./gradlew runAprilTagSim + ``` + + Or with custom arguments: + ```bash + ./gradlew runAprilTagSim --args="localhost my_topic 8.0 30.0" + ``` + +## Configuration + +### NetworkTables Topic Name + +The topic name must match what your Orin mechanism expects: + +**In simulation** (`RobotMain.java`): +```java +aprilTagSimulator = new AprilTagSimulator( + "apriltag_detections", // ← This topic name + 5.0, 20.0 +); +``` + +**In robot code** (wherever you create `GetOrinRawValue`): +```java +GetOrinRawValue orinReader = new GetOrinRawValue( + "apriltag_detections", // ← Must match! + 0.1 // covariance +); +``` + +### Field Tag Positions + +Update the field tag positions in `AprilTagSimulator.initializeFieldTags()`: + +```java +private void initializeFieldTags() { + // Tag ID, X (meters), Y (meters), Z (meters - height) + fieldTags.add(new FieldAprilTag(1, 0.0, 0.0, 0.5)); + fieldTags.add(new FieldAprilTag(2, 5.0, 0.0, 0.5)); + // ... add all field tags here +} +``` + +**Important**: These positions should match the actual AprilTag layout for your game's field. You can usually find these in the FRC game manual or WPILib's `AprilTagFieldLayout`. + +### Detection Parameters + +**Detection Range**: Maximum distance (in meters) at which tags can be detected +```java +aprilTagSimulator = new AprilTagSimulator( + "apriltag_detections", + 5.0, // ← Detection range in meters + 20.0 +); +``` + +**Publish Rate**: How often (Hz) to publish detections +```java +aprilTagSimulator = new AprilTagSimulator( + "apriltag_detections", + 5.0, + 20.0 // ← 20 Hz = every 50ms +); +``` + +## How It Works + +1. **Every simulation tick** (`simulationPeriodic`): + - `aprilTagSimulator.simulateAprilTags(currentTime)` is called + +2. **Inside `simulateAprilTags()`**: + - Reads robot position from `ProgramInterface.robotPosition` (x, y, heading) + - For each field tag: + - Calculates distance from robot to tag + - If within detection range: + - Transforms tag position from field frame to robot frame + - Adds to detection array: `[timestamp, tagId, x, y, z]` + - Publishes array to NetworkTables + +3. **Your robot code**: + - `Orin` mechanism calls `GetApriltagPoseData.getAllTags()` + - Which calls `GetOrinRawValue.getRawPoseData()` + - Which reads from NetworkTables + - Returns list of `TimestampedApriltag` objects + - Your code uses these for localization/targeting/etc. + +## Testing + +### Verify NetworkTables Communication + +1. Start simulation with AprilTag simulator +2. Open **OutlineViewer** (WPILib tool): + ```bash + ~/wpilib/2025/tools/OutlineViewer.jar + ``` +3. Connect to `localhost` +4. Look for `/SmartDashboard/apriltag_detections` +5. You should see arrays of doubles being published + +### Debug Output + +The simulator prints debug info when tags are detected: +``` +[AprilTagSim] Published 2 tags at t=5.43 +``` + +### Test with Specific Tags + +For testing, you can bypass the automatic detection and inject specific tags: +```java +// In your test code: +aprilTagSimulator.simulateSpecificTag( + currentTime, // timestamp + 1, // tag ID + 2.0, 0.5, 0.3 // x, y, z in robot frame +); +``` + +## Troubleshooting + +### "No tags detected" +- Check that field tags are configured in `initializeFieldTags()` +- Verify detection range is large enough +- Check that robot is moving in simulation (look at `ProgramInterface.robotPosition`) + +### "GetOrinRawValue throws ValueNotFoundOnTableError" +- Verify topic names match between simulator and `GetOrinRawValue` +- Check NetworkTables connection (use OutlineViewer) +- Make sure simulator is running and publishing + +### "Wrong tag positions" +- Verify field tag coordinates are correct (check field layout) +- Check that coordinate frame is correct (WPILib uses blue alliance origin) +- Make sure robot position is being set correctly in simulation + +## Future Enhancements + +Potential improvements to make simulation more realistic: + +1. **Camera FOV**: Only detect tags within camera field of view +2. **Occlusion**: Don't detect tags behind obstacles +3. **Noise**: Add realistic sensor noise to detections +4. **Latency**: Add network latency to simulate real Orin→RoboRIO delay +5. **Multiple cameras**: Simulate multiple cameras with different poses +6. **Tag orientation**: Include full 6DOF pose (currently only translation) + +## See Also + +- `src/main/java/com/team766/simulator/AprilTagSimulator.java` - Main simulator +- `src/main/java/com/team766/simulator/StandaloneAprilTagSimulator.java` - Standalone version +- `src/main/java/com/team766/orin/GetOrinRawValue.java` - NetworkTables reader +- `src/main/java/com/team766/orin/GetApriltagPoseData.java` - Data parser +- `src/main/java/com/team766/robot/reva/mechanisms/Orin.java` - Orin mechanism diff --git a/simConfig.txt b/simConfig.txt index 6d76a7d03..932efbe02 100644 --- a/simConfig.txt +++ b/simConfig.txt @@ -1,4 +1,5 @@ { + "robotConfigurator": "com.team766.robot.reva_2025.Robot", "simulationMode": "VrConnector", "drive": { "leftMotor": { diff --git a/src/main/java/com/team766/hal/wpilib/RobotMain.java b/src/main/java/com/team766/hal/wpilib/RobotMain.java index 88465905f..4c149e59b 100755 --- a/src/main/java/com/team766/hal/wpilib/RobotMain.java +++ b/src/main/java/com/team766/hal/wpilib/RobotMain.java @@ -35,6 +35,7 @@ public class RobotMain extends LoggedRobot { private GenericRobotMain robot; private SimulatorInterface simulator; + private com.team766.simulator.AprilTagSimulator aprilTagSimulator; public static void main(final String... args) { Supplier supplier = @@ -201,6 +202,18 @@ enum SimulationMode { simulator.setResetHandler(() -> robot.resetAutonomousMode("simulation reset")); // Do an initial step here to flush any needed state initialization. simulator.prepareStep(); + + // Initialize AprilTag simulator + // Topic names must match what Vision.java expects + String[] cameraTopics = {"left_back", "left_front", "right_back", "right_front"}; + aprilTagSimulator = + new com.team766.simulator.AprilTagSimulator( + cameraTopics, // NetworkTables topic names (matches Vision.java) + 5.0, // Detection range in meters + 20.0 // Publish rate in Hz + ); + com.team766.logging.Logger.get(Category.FRAMEWORK) + .logRaw(Severity.INFO, "AprilTag simulator initialized"); } catch (Exception exc) { exc.printStackTrace(); LoggerExceptionUtils.logException(exc); @@ -228,6 +241,11 @@ public void simulationPeriodic() { } } DriverStationSim.notifyNewData(); + + // Update AprilTag simulator with current simulation time + if (aprilTagSimulator != null) { + aprilTagSimulator.simulateAprilTags(ProgramInterface.simulationTime); + } } catch (Exception exc) { exc.printStackTrace(); LoggerExceptionUtils.logException(exc); diff --git a/src/main/java/com/team766/simulator/AprilTagSimulator.java b/src/main/java/com/team766/simulator/AprilTagSimulator.java new file mode 100644 index 000000000..84b2171f9 --- /dev/null +++ b/src/main/java/com/team766/simulator/AprilTagSimulator.java @@ -0,0 +1,177 @@ +package com.team766.simulator; + +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +/** + * Simulates an Orin coprocessor publishing AprilTag detections to NetworkTables. + * Simplified version - publishes same data to multiple camera topics without worrying + * about camera extrinsics or field of view. + * + * The Orin publishes AprilTag data as double arrays with the format: + * [timestamp, tagId, x, y, z, timestamp, tagId, x, y, z, ...] + * + * Usage: + * - Create simulator with camera topic names + * - Call simulateAprilTags() periodically during simulation + */ +public class AprilTagSimulator { + + private final NetworkTableInstance ntInstance; + private final NetworkTable table; + private final Map publishers = new HashMap<>(); + + // Configuration + private final double detectionRange; // meters + private final double publishRateHz; + + // State + private double lastPublishTime = 0.0; + + /** + * Represents an AprilTag on the field with a known position. + */ + public static class FieldAprilTag { + public final int id; + public final double x; // meters + public final double y; // meters + public final double z; // meters (height) + + public FieldAprilTag(int id, double x, double y, double z) { + this.id = id; + this.x = x; + this.y = y; + this.z = z; + } + } + + // Known AprilTag locations on the field + private final List fieldTags = new ArrayList<>(); + + /** + * Create AprilTag simulator. + * + * @param cameraTopics List of NetworkTables topic names (e.g., ["left_back", "right_front"]) + * @param detectionRangeMeters Maximum detection range in meters + * @param publishRateHz How often to publish detections + */ + public AprilTagSimulator( + String[] cameraTopics, double detectionRangeMeters, double publishRateHz) { + this.detectionRange = detectionRangeMeters; + this.publishRateHz = publishRateHz; + + ntInstance = NetworkTableInstance.getDefault(); + table = ntInstance.getTable("/SmartDashboard"); + + // Create publishers for each camera topic + for (String topic : cameraTopics) { + DoubleArrayPublisher publisher = table.getDoubleArrayTopic(topic).publish(); + publishers.put(topic, publisher); + System.out.printf("[AprilTagSim] Publishing to topic: %s%n", topic); + } + + // Initialize with some example tags (update these for your field) + initializeFieldTags(); + } + + /** + * Configure the known AprilTag positions on the field. + * You should update this based on your actual field layout. + */ + private void initializeFieldTags() { + // Example: Add some test tags + // Format: (tagId, x, y, z) + fieldTags.add(new FieldAprilTag(1, 0.0, 0.0, 0.5)); // Tag 1 at origin + fieldTags.add(new FieldAprilTag(2, 5.0, 0.0, 0.5)); // Tag 2 at 5m + fieldTags.add(new FieldAprilTag(3, 5.0, 5.0, 0.5)); // Tag 3 at corner + fieldTags.add(new FieldAprilTag(6, 0.0, 5.0, 0.5)); // Tag 6 at corner + // Add more tags based on your field layout + } + + /** + * Add a field tag dynamically (useful for testing). + */ + public void addFieldTag(int id, double x, double y, double z) { + fieldTags.add(new FieldAprilTag(id, x, y, z)); + } + + /** + * Simulates AprilTag detections based on robot position. + * Publishes the same data to all camera topics. + * Call this periodically during simulation (e.g., every 20ms in simulationPeriodic). + * + * @param currentTime Current simulation time in seconds + */ + public void simulateAprilTags(double currentTime) { + // Rate limiting - only publish at the configured rate + double timeSinceLastPublish = currentTime - lastPublishTime; + if (timeSinceLastPublish < (1.0 / publishRateHz)) { + return; + } + lastPublishTime = currentTime; + + // Get robot position from simulation + double robotX = ProgramInterface.robotPosition.x; + double robotY = ProgramInterface.robotPosition.y; + double robotHeading = ProgramInterface.robotPosition.heading; // degrees + + // Find all tags within detection range + List detectionData = new ArrayList<>(); + + for (FieldAprilTag tag : fieldTags) { + double dx = tag.x - robotX; + double dy = tag.y - robotY; + double distance = Math.sqrt(dx * dx + dy * dy); + + // Only detect tags within range + if (distance <= detectionRange) { + // Convert tag position from field frame to robot frame + // This simulates what the camera would see + double angleToTag = Math.atan2(dy, dx) - Math.toRadians(robotHeading); + double relativeX = distance * Math.cos(angleToTag); + double relativeY = distance * Math.sin(angleToTag); + double relativeZ = tag.z; // Simplified - doesn't account for robot tilt + + // Add to detection array: [timestamp, tagId, x, y, z] + detectionData.add(currentTime); + detectionData.add((double) tag.id); + detectionData.add(relativeX); + detectionData.add(relativeY); + detectionData.add(relativeZ); + } + } + + // Publish to all camera topics + if (!detectionData.isEmpty()) { + double[] array = detectionData.stream().mapToDouble(Double::doubleValue).toArray(); + for (Map.Entry entry : publishers.entrySet()) { + entry.getValue().set(array); + } + + // Debug output + System.out.printf( + "[AprilTagSim] Published %d tags at t=%.2f (robot: %.2f, %.2f, %.1f°)%n", + detectionData.size() / 5, currentTime, robotX, robotY, robotHeading); + } else { + // Publish empty array when no tags detected + double[] emptyArray = new double[0]; + for (DoubleArrayPublisher publisher : publishers.values()) { + publisher.set(emptyArray); + } + } + } + + /** + * Stop publishing and clean up resources. + */ + public void close() { + for (DoubleArrayPublisher publisher : publishers.values()) { + publisher.close(); + } + } +}