Add auto path preloading

- Refactor recording to use commands

src/main/java/frc4388/robot/Robot.java

@@ -5,22 +5,23 @@
 package frc4388.robot;
 
 import java.io.File;
-import java.io.IOException;
-
+import java.nio.file.Files;
+import java.nio.file.Path;
 import java.util.logging.Level;
 import java.util.logging.Logger;
+import java.util.stream.StreamSupport;
+
+import com.diffplug.common.base.Errors;
 
-import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc4388.utility.PathPlannerUtil;
-import frc4388.utility.RobotLogger;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc4388.utility.RobotTime;
-import frc4388.utility.PathPlannerUtil.Path;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -61,6 +62,33 @@ public class Robot extends TimedRobot {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    // addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod);
+    SmartDashboard.putData(CommandScheduler.getInstance());
+    SmartDashboard.putData("JVM Memory", new RunCommand(() -> {}) {
+      @Override public boolean runsWhenDisabled() { return true; }
+      @Override public String getName() {
+        if (isScheduled()) {
+          Runtime runtime = Runtime.getRuntime();
+          long totalMemory = runtime.totalMemory() / 1_000_000;
+          long freeMemory = runtime.freeMemory() / 1_000_000;
+          long maxMemory = runtime.maxMemory() / 1_000_000;
+          return totalMemory - freeMemory + " MB / " + totalMemory + " MB / " + maxMemory + " MB";
+        }
+        return "Not Running";
+      }
+    });
+    SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> {}) {
+      @Override public boolean runsWhenDisabled() { return true; }
+      @Override public String getName() {
+        if (isScheduled()) {
+          File deploy = Filesystem.getDeployDirectory();
+          long usedSpace = Errors.suppress().getWithDefault(() -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(), 0l) / 1_000_000;
+          long usableSpace = deploy.getUsableSpace() / 1_000_000;
+          return usedSpace + " MB / " + usableSpace + " MB";
+        }
+        return "Not Running";
+      }
+    });
   }
 
   /**
@@ -94,26 +122,13 @@ public class Robot extends TimedRobot {
     LOGGER.fine("disabledInit()");
     m_robotTime.endMatchTime();
     if (isTest()) {
-      try {
-        // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
-        Path path = RobotLogger.getInstance().createPath(0.1, 0.1, false);
-        String pathPath = "recording." + System.currentTimeMillis() + ".path";
-        File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve(pathPath).toFile();
-        outputFile.createNewFile();
-        path.write(outputFile);
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "Recorded path to {0} in the deploy directory on the RoboRIO", pathPath);
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        // LOGGER.finest(path::toString);
-      } catch (IOException e) {
-        e.printStackTrace();
-      }
+      // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
+      File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("recording." + System.currentTimeMillis() + ".path").toFile();
+      if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
+        m_robotContainer.createPath(null, null, false).write(outputFile);
+        LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
+      } else
+        LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
     }
   }
 
@@ -130,12 +145,7 @@ public class Robot extends TimedRobot {
   @Override
   public void autonomousInit() {
     LOGGER.fine("autonomousInit()");
-    try {
-      m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-    } catch (IOException e) {
-      // TODO Auto-generated catch block
-      e.printStackTrace();
-    }
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
@@ -176,6 +186,7 @@ public class Robot extends TimedRobot {
 
   @Override
   public void testInit() {
+    CommandScheduler.getInstance().cancelAll();
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -6,14 +6,27 @@ package frc4388.robot;
 
 import java.io.File;
 import java.io.IOException;
+import java.nio.file.FileSystem;
+import java.nio.file.FileSystems;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.nio.file.StandardWatchEventKinds;
+import java.nio.file.WatchEvent;
+import java.nio.file.WatchKey;
+import java.nio.file.WatchService;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Comparator;
+import java.util.List;
 import java.util.Optional;
+import java.util.function.Function;
+import java.util.logging.Level;
 import java.util.logging.Logger;
+import java.util.regex.Matcher;
+import java.util.regex.Pattern;
 
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.diffplug.common.base.Errors;
+import com.diffplug.common.base.Errors.Plugins.Log;
 import com.pathplanner.lib.PathPlanner;
 import com.pathplanner.lib.PathPlannerTrajectory;
 import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
@@ -23,12 +36,23 @@ import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.command.PrintCommand;
+import edu.wpi.first.wpilibj.shuffleboard.EventImportance;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.NotifierCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -38,6 +62,9 @@ import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.PathPlannerUtil;
+import frc4388.utility.SendableRunChooser;
+import frc4388.utility.PathPlannerUtil.Path.Waypoint;
 import frc4388.utility.controller.DeadbandedXboxController;
 
 /**
@@ -48,6 +75,7 @@ import frc4388.utility.controller.DeadbandedXboxController;
  * commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+  private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getName());
   /* RobotMap */
   private final RobotMap m_robotMap = new RobotMap();
 
@@ -55,17 +83,28 @@ public class RobotContainer {
   private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
       m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
 
-  private final TalonFX m_testMotor = new TalonFX(23);
-
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
   /* Controllers */
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
 
+  private String loadedPathName = "";
+  private PathPlannerTrajectory loadedPathTrajectory = null;
+  private static final Function<CharSequence, String> pathExtensionRemover = ((Function<CharSequence, Matcher>) Pattern
+      .compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
+
+  private final SendableRunChooser<File> autoChooser = new SendableRunChooser<>(selectedAuto -> {
+    if (selectedAuto != null && !selectedAuto.equals(loadedPathName)) {
+      loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto), 5.5, 50);
+      loadedPathName = selectedAuto;
+    }
+  });
+
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
+  @SuppressWarnings("unchecked")
   public RobotContainer() {
     configureButtonBindings();
     /* Default Commands */
@@ -77,14 +116,67 @@ public class RobotContainer {
             getDriverController().getRightX(),
             getDriverController().getRightY(),
             true),
-            m_robotSwerveDrive));
+            m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
 
     // continually sends updates to the Blinkin LED controller to keep the lights on
-    m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
+    m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand"));
+    try (WatchService watcher = FileSystems.getDefault().newWatchService()) {
+      WatchKey watchKey = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").register(
+          FileSystems.getDefault().newWatchService(),
+          StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
+          StandardWatchEventKinds.ENTRY_DELETE);
+      new NotifierCommand(() -> {}, TimedRobot.kDefaultPeriod) {
+        @Override
+        public void execute() {
+          var selectedAuto = autoChooser.getSelected();
+          if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName)) {
+            setName("Path Watcher: Loading Path");
+            // loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5, 50);
+            loadedPathName = selectedAuto.getName();
+          }
+          if (!watchKey.pollEvents().isEmpty()) {
+            updateAutoChooser();
+            LOGGER.info("Updated autonomous chooser.");
+          }
+          if (!watchKey.reset())
+            LOGGER.severe("File watch key invalid.");
+          setName("Path Watcher: Waiting");
+        }
 
-    m_testMotor.set(TalonFXControlMode.PercentOutput, 0);
+        @Override
+        public boolean runsWhenDisabled() {
+          return true;
+        }
+      }.schedule();
+      pathLoaderCommand.schedule();
+    } catch (IOException exception) {
+      LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
+    }
+    updateAutoChooser();
+    recordInit();
   }
 
+  private Command pathLoaderCommand = new CommandBase() {
+    @Override
+    public void execute() {
+      var selectedAuto = autoChooser.getSelected();
+      if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName)) {
+        setName("LOADING PATH");
+        Thread thread = new Thread(() -> {
+          loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5, 50);
+          setName("Path loader waiting");
+        }, "Path Loader Thread");
+        thread.start();
+      }
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+      return true;
+    }
+  }.withName("Path loader");
+  private Thread pathLoaderThread = new Thread();
+
   /**
    * Use this method to define your button->command mappings. Buttons can be
    * created by instantiating a {@link GenericHID} or one of its subclasses
@@ -108,7 +200,7 @@ public class RobotContainer {
         .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
 
     new JoystickButton(getDriverController(), XboxController.Button.kA.value)
-      .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
+        .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
 
     /* Operator Buttons */
     // activates "Lit Mode"
@@ -118,59 +210,39 @@ public class RobotContainer {
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
   }
 
-  public Command[] buildAuto(double maxVel, double maxAccel, Object... inputs) {
-    ArrayList<Command> commands = new ArrayList<>();
-    commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
-
-    PIDController xController = SwerveDriveConstants.X_CONTROLLER;
-    PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
-    ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
-    thetaController.enableContinuousInput(-Math.PI, Math.PI);
-
-    // parse input
-    for (int i = 0; i < inputs.length; i++) {
-      if (inputs[i] instanceof String) {
-        PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
-        PathPlannerState initState = (PathPlannerState) traj.sample(0);
-
-        commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
-        commands.add(new PPSwerveControllerCommand(traj, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive));
-      } else if (inputs[i] instanceof Command)
-        commands.add((Command) inputs[i]);
-    }
-
-    commands.add(new InstantCommand(m_robotSwerveDrive::stopModules));
-    Command[] ret = new Command[commands.size()];
-    ret = commands.toArray(ret);
-    return ret;
-  }
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
    * @return the command to run in autonomous
-   * @throws IOException
    */
-  public Command getAutonomousCommand() throws IOException {
-    // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
+  public Command getAutonomousCommand() {
+    var selectedAuto = autoChooser.getSelected();
+    if (loadedPathTrajectory != null) {
+      // if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName))
+      // loadedPathTrajectory =
+      // PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5,
+      // 50);
+      PIDController xController = SwerveDriveConstants.X_CONTROLLER;
+      PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
+      ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
+      thetaController.enableContinuousInput(-Math.PI, Math.PI);
 
-    // Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down");
-    // SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands);
-    // return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2)))));
-    Optional<File> recordedAuto = Arrays.stream(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").toFile().listFiles()).filter(f -> f.getName().startsWith("recording")).max(Comparator.comparingLong(File::lastModified));
-    if (recordedAuto.isPresent()) {
-      String name = recordedAuto.orElseThrow().getName().replace(".path", "");
-      Logger.getLogger(this.getClass().getName()).fine(name);
-      return new SequentialCommandGroup(buildAuto(1.0, 1.0, name));
+      PathPlannerState initialState = loadedPathTrajectory.getInitialState();
+      Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
+      return new SequentialCommandGroup(
+          new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
+          new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
+          new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry,
+              m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
+              m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
+          new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
+    } else {
+      Logger.getLogger(this.getClass().getName()).severe("No auto selected.");
+      return new RunCommand(() -> {
+      }).withName("No Autonomous Path");
     }
-    Logger.getLogger(this.getClass().getName()).severe("No recorded autos found.");
-    return new InstantCommand();
-    // return new PrintCommand("No recorded autos found.");
   }
 
-  /**
-   * Add your docs here.
-   */
   public XboxController getDriverController() {
     return m_driverXbox;
   }
@@ -183,10 +255,67 @@ public class RobotContainer {
     m_robotSwerveDrive.resetOdometry(pose);
   }
 
-  /**
-   * Add your docs here.
-   */
   public XboxController getOperatorController() {
     return m_operatorXbox;
   }
+
+  private void updateAutoChooser() {
+    Arrays.stream(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").toFile().listFiles())
+        .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
+        .forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
+    SmartDashboard.putData("Auto Chooser", autoChooser);
+  }
+
+  private final List<Waypoint> pathPoints = new ArrayList<>();
+
+  public void recordInit() {
+    SmartDashboard.putData("Recording",
+        new RunCommand(this::recordPeriodic) {
+          @Override
+          public void end(boolean interupted) {
+            new InstantCommand(RobotContainer.this::saveRecording) {
+              @Override
+              public boolean runsWhenDisabled() {
+                return true;
+              }
+            }.withName("Save Recording").schedule();
+          }
+        }.withName("Record Path (Cancel to Save)"));
+  }
+
+  private void saveRecording() {
+    // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
+    File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner")
+        .resolve("recording." + System.currentTimeMillis() + ".path").toFile();
+    if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
+      createPath(null, null, false).write(outputFile);
+      autoChooser.setDefaultOption(outputFile.getName(), outputFile);
+      LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
+    } else
+      LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
+  }
+
+  public void recordPeriodic() {
+    Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
+    Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
+    Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond,
+        m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
+    Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, velocity.getNorm(),
+        false);
+    pathPoints.add(waypoint);
+  }
+
+  public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
+    PathPlannerUtil.Path path = new PathPlannerUtil.Path();
+    for (int i = 0; i < pathPoints.size() - 2; i++)
+      pathPoints.get(i).nextControl = pathPoints.get(i + 1).anchorPoint;
+    for (int i = 1; i < pathPoints.size() - 1; i++)
+      pathPoints.get(i).prevControl = pathPoints.get(i - 1).anchorPoint;
+    path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new));
+    path.maxVelocity = Optional.ofNullable(maxVelocity);
+    path.maxAcceleration = Optional.ofNullable(maxAcceleration);
+    path.isReversed = Optional.ofNullable(isReversed);
+    pathPoints.clear();
+    return path;
+  }
 }

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -4,8 +4,8 @@
 
 package frc4388.robot.subsystems;
 
-import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
+import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -23,8 +23,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
-import frc4388.utility.RobotLogger;
-import frc4388.utility.PathPlannerUtil.Path.Waypoint.Point;
 
 public class SwerveDrive extends SubsystemBase {
 
@@ -57,7 +55,7 @@ public class SwerveDrive extends SubsystemBase {
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   public boolean ignoreAngles;
   public Rotation2d rotTarget = new Rotation2d();;
-  private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
+  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
   private final Field2d m_field = new Field2d();
 
@@ -160,23 +158,7 @@ public class SwerveDrive extends SubsystemBase {
 
   @Override
   public void periodic() {
-    // System.err.println(m_gyro.getFusedHeading() +"    aaa");
     updateOdometry();
-    // m_gyro.setFusedHeadingToCompass();
-    // m_gyro.setYawToCompass();
-    Pose2d pos = m_poseEstimator.getEstimatedPosition();
-    RobotLogger.getInstance().put(
-      /* anchorPointX */ pos.getX(),
-      /* anchorPointY */ pos.getY(),
-      /* prevControlX */ pos.getX(),
-      /* prevControlY */ pos.getY(),
-      /* nextControlX */ pos.getX(),
-      /* nextControlY */ pos.getY(),
-      /* holonomicAngle */ m_gyro.getRotation2d().getDegrees(),
-      /* isReversal */ false,
-      /* velOverride */ null,
-      /* isLocked */ false
-    );
     SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
     SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
     SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
@@ -186,8 +168,7 @@ public class SwerveDrive extends SubsystemBase {
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-
-    // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
+    m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
     super.periodic();
   }
 

src/main/java/frc4388/utility/AnsiLogging.java

@@ -19,6 +19,10 @@ import java.util.logging.Logger;
 import org.fusesource.jansi.Ansi;
 import org.fusesource.jansi.Ansi.Attribute;
 import org.fusesource.jansi.Ansi.Color;
+
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 import org.fusesource.jansi.AnsiConsole;
 
 public class AnsiLogging extends ConsoleHandler {
@@ -35,7 +39,7 @@ public class AnsiLogging extends ConsoleHandler {
       // Replaces standard output streams with org.fusesource.jansi.AnsiPrintStreams.
       AnsiConsole.systemInstall();
       // Replaces standard output stream with java.util.logging.Logger.
-      System.setOut(printStreamLogger(Logger.getGlobal(), Level.INFO));
+      System.setOut(printStreamLogger(Logger.getGlobal(), Level.ALL));
       // Replaces standard error output stream with java.util.logging.Logger.
       System.setErr(printStreamLogger(Logger.getGlobal(), Level.SEVERE));
     } catch (IOException exception) {
@@ -66,6 +70,7 @@ public class AnsiLogging extends ConsoleHandler {
         } else
           source = logRecord.getLoggerName();
         String message = formatMessage(logRecord);
+        boolean multiline = message.contains("\n");
         String throwable = "";
         if (logRecord.getThrown() != null) {
           StringWriter sw = new StringWriter();
@@ -74,6 +79,7 @@ public class AnsiLogging extends ConsoleHandler {
             logRecord.getThrown().printStackTrace(pw);
           }
           throwable = sw.toString();
+          multiline = true;
         }
         Ansi ansi;
         if (logRecord.getLevel() == Level.SEVERE) ansi = ansi().fgBright(Color.RED);
@@ -84,7 +90,7 @@ public class AnsiLogging extends ConsoleHandler {
         else if (logRecord.getLevel() == Level.FINER) ansi = ansi().fg(Color.MAGENTA);
         else if (logRecord.getLevel() == Level.FINEST) ansi = ansi().fgBright(Color.BLACK);
         else ansi = ansi().fg(Color.DEFAULT);
-        String format = ansi.bold().a(Attribute.UNDERLINE).a("%1$tb %1$td, %1$tY %1$tl:%1$tM:%1$tS %1$Tp %2$s %4$s:").boldOff().a(Attribute.UNDERLINE_OFF).a("%n%5$s%6$s").reset().a("%n").toString();
+        String format = ansi.bold().a(Attribute.UNDERLINE).a("%1$tb %1$td, %1$tY %1$tl:%1$tM:%1$tS %1$Tp %2$s %4$s:").boldOff().a(Attribute.UNDERLINE_OFF).a(multiline ? "%n%5$s%6$s" : " %5$s%6$s").reset().a("%n").toString();
         return String.format(format, zdt, source, logRecord.getLoggerName(), logRecord.getLevel().getLocalizedName(), message, throwable);
       }
     };

src/main/java/frc4388/utility/PathPlannerUtil.java

@@ -2,53 +2,26 @@ package frc4388.utility;
 
 import java.io.File;
 import java.util.Optional;
+
 import com.diffplug.common.base.Errors;
+import com.fasterxml.jackson.annotation.JsonInclude.Include;
 import com.fasterxml.jackson.databind.ObjectMapper;
 import com.fasterxml.jackson.datatype.jdk8.Jdk8Module;
 
-public class PathPlannerUtil {
+import edu.wpi.first.math.geometry.Translation2d;
+
+public final class PathPlannerUtil {
   public static final class Path {
-    public Optional<Waypoint>[] waypoints;
-
-    public static final class Waypoint {
-      public Waypoint() {}
-      public Waypoint(Double anchorPointX, Double anchorPointY, Double prevControlX, Double prevControlY, Double nextControlX, Double nextControlY, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) {
-        this.anchorPoint = anchorPointX == null && anchorPointY == null ? Optional.empty() : Optional.of(Point.of(anchorPointX, anchorPointY));
-        this.prevControl = prevControlX == null && prevControlY == null ? Optional.empty() : Optional.of(Point.of(prevControlX, prevControlY));
-        this.nextControl = nextControlX == null && nextControlY == null ? Optional.empty() : Optional.of(Point.of(nextControlX, nextControlY));
-        this.holonomicAngle = Optional.ofNullable(holonomicAngle);
-        this.isReversal = Optional.ofNullable(isReversal);
-        this.velOverride = Optional.ofNullable(velOverride);
-        this.isLocked = Optional.ofNullable(isLocked);
-      }
-      public Optional<Point> anchorPoint;
-      public Optional<Point> prevControl;
-      public Optional<Point> nextControl;
-
-      public static final class Point {
-        public Point() {}
-        public static Point of(Double x, Double y) {
-          Point point = new Point();
-          point.x = Optional.ofNullable(x);
-          point.y = Optional.ofNullable(y);
-          return point;
-        }
-        public Optional<Double> x;
-        public Optional<Double> y;
-      }
-
-      public Optional<Double> holonomicAngle;
-      public Optional<Boolean> isReversal;
-      public Optional<Double> velOverride;
-      public Optional<Boolean> isLocked;
-    }
-
+    public Optional<Waypoint[]> waypoints;
     public Optional<Double> maxVelocity;
     public Optional<Double> maxAcceleration;
     public Optional<Boolean> isReversed;
 
     private static final ObjectMapper objectMapper = new ObjectMapper();
-    static { objectMapper.registerModule(new Jdk8Module()); }
+    static {
+      objectMapper.registerModule(new Jdk8Module());
+      objectMapper.setSerializationInclusion(Include.NON_EMPTY);
+    }
 
     public static Path read(File src) {
       return Errors.log().getWithDefault(() -> objectMapper.readValue(src, Path.class), null);
@@ -58,9 +31,27 @@ public class PathPlannerUtil {
       Errors.log().run(() -> objectMapper.writeValue(resultFile, this));
     }
 
-    @Override
-    public String toString() {
-      return Errors.log().getWithDefault(() -> objectMapper.writeValueAsString(this), super.toString());
+    public static final class Waypoint {
+      public Optional<Translation2d> anchorPoint;
+      public Optional<Translation2d> prevControl;
+      public Optional<Translation2d> nextControl;
+      public Optional<Double> holonomicAngle;
+      public Optional<Boolean> isReversal;
+      public Optional<Double> velOverride;
+      public Optional<Boolean> isLocked;
+
+      public Waypoint() {
+      }
+
+      public Waypoint(Translation2d anchorPoint, Translation2d prevControl, Translation2d nextControl, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) {
+        this.anchorPoint = Optional.ofNullable(anchorPoint);
+        this.prevControl = Optional.ofNullable(prevControl);
+        this.nextControl = Optional.ofNullable(nextControl);
+        this.holonomicAngle = Optional.ofNullable(holonomicAngle);
+        this.isReversal = Optional.ofNullable(isReversal);
+        this.velOverride = Optional.ofNullable(velOverride);
+        this.isLocked = Optional.ofNullable(isLocked);
+      }
     }
   }
 }

src/main/java/frc4388/utility/RobotGyro.java.old

@@ -1,245 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.utility;
-
-import com.ctre.phoenix.sensors.PigeonIMU;
-import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
-import com.kauailabs.navx.frc.AHRS;
-
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.util.sendable.Sendable;
-import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-
-/**
- * Gyro class that allows for interchangeable use between a pigeon and a navX
- */
-public class RobotGyro implements Gyro, PIDSource, Sendable {
-  private RobotTime m_robotTime = RobotTime.getInstance();
-
-  private PigeonIMU m_pigeon = null;
-  private AHRS m_navX = null;
-  public boolean m_isGyroAPigeon; // true if pigeon, false if navX
-
-  private double m_lastPigeonAngle;
-  private double m_deltaPigeonAngle;
-
-  /**
-   * Creates a Gyro based on a pigeon
-   * 
-   * @param gyro the gyroscope to use for Gyro
-   */
-  public RobotGyro(PigeonIMU gyro) {
-    m_pigeon = gyro;
-    m_isGyroAPigeon = true;
-  }
-
-  /**
-   * Creates a Gyro based on a navX
-   * 
-   * @param gyro the gyroscope to use for Gyro
-   */
-  public RobotGyro(AHRS gyro) {
-    m_navX = gyro;
-    m_isGyroAPigeon = false;
-  }
-
-  /**
-   * Run in periodic if you are using a pigeon. Updates a delta angle so that it
-   * can calculate getRate(). Note
-   * that the getRate() method for a navX will likely be much more accurate than
-   * for a pigeon.
-   */
-  public void updatePigeonDeltas() {
-    double currentPigeonAngle = getAngle();
-    m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
-    m_lastPigeonAngle = currentPigeonAngle;
-  }
-
-  /**
-   * <p>
-   * NavX:
-   * <p>
-   * Calibrate the gyro by running for a number of samples and computing the
-   * center value. Then use
-   * the center value as the Accumulator center value for subsequent measurements.
-   * It's important to
-   * make sure that the robot is not moving while the centering calculations are
-   * in progress, this
-   * is typically done when the robot is first turned on while it's sitting at
-   * rest before the
-   * competition starts.
-   * 
-   * <p>
-   * Pigeon:
-   * <p>
-   * Calibrate the gyro by collecting data at a range of tempuratures. Allow
-   * pigeon to cool, then boot
-   * into calibration mode. For faster calibration, use a heat lamp to heat up the
-   * pigeon. Once the pigeon
-   * has seen a reasonable range of tempuratures, it will exit calibration mode.
-   * It's important to
-   * make sure that the robot is not moving while the tempurature calculations are
-   * in progress, this
-   * is typically done when the robot is first turned on while it's sitting at
-   * rest before the
-   * competition starts.
-   */
-  @Override
-  public void calibrate() {
-    if (m_isGyroAPigeon)
-      m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
-    else
-      m_navX.calibrate();
-  }
-
-  @Override
-  public void reset() {
-    if (m_isGyroAPigeon)
-      m_pigeon.setYaw(0);
-    else
-      m_navX.reset();
-  }
-
-  /**
-   * Get Yaw, Pitch, and Roll data.
-   *
-   * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
-   *         Yaw is within [-368,640, +368,640] degrees.
-   *         Pitch is within [-90,+90] degrees.
-   *         Roll is within [-90,+90] degrees.
-   */
-  private double[] getPigeonAngles() {
-    double[] angles = new double[3];
-    m_pigeon.getYawPitchRoll(angles);
-    return angles;
-  }
-
-  @Override
-  public double getAngle() {
-    if (m_isGyroAPigeon) {
-      return getPigeonAngles()[0];
-    } else {
-      return m_navX.getAngle();
-    }
-  }
-
-  /**
-   * Gets an absolute heading of the robot
-   * 
-   * @return heading from -180 to 180 degrees
-   */
-  public double getHeading() {
-    return getHeading(getAngle());
-  }
-
-  /**
-   * Gets an absolute heading of the robot
-   * 
-   * @return heading from -180 to 180 degrees
-   */
-  public double getHeading(double angle) {
-    return Math.IEEEremainder(angle, 360);
-  }
-
-  /**
-   * Returns the current pitch value (in degrees, from -90 to 90)
-   * reported by the sensor. Pitch is a measure of rotation around
-   * the Y Axis.
-   * 
-   * @return The current pitch value in degrees (-90 to 90).
-   */
-  public double getPitch() {
-    if (m_isGyroAPigeon) {
-      return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
-    } else {
-      return MathUtil.clamp(m_navX.getPitch(), -90, 90);
-    }
-  }
-
-  /**
-   * Returns the current roll value (in degrees, from -90 to 90)
-   * reported by the sensor. Roll is a measure of rotation around
-   * the X Axis.
-   * 
-   * @return The current roll value in degrees (-90 to 90).
-   */
-  public double getRoll() {
-    if (m_isGyroAPigeon) {
-      return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
-    } else {
-      return MathUtil.clamp(m_navX.getRoll(), -90, 90);
-    }
-  }
-
-  @Override
-  public double getRate() {
-    if (m_isGyroAPigeon) {
-      return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
-    } else {
-      return m_navX.getRate();
-    }
-  }
-
-  public PigeonIMU getPigeon() {
-    return m_pigeon;
-  }
-
-  public AHRS getNavX() {
-    return m_navX;
-  }
-
-  @Override
-  public void close() throws Exception {
-
-  }
-
-  // Begin old GyroBase class
-  private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
-
-  /**
-   * Set which parameter of the gyro you are using as a process control variable.
-   * The Gyro class
-   * supports the rate and displacement parameters
-   *
-   * @param pidSource An enum to select the parameter.
-   */
-  @Override
-  public void setPIDSourceType(PIDSourceType pidSource) {
-    m_pidSource = pidSource;
-  }
-
-  @Override
-  public PIDSourceType getPIDSourceType() {
-    return m_pidSource;
-  }
-
-  /**
-   * Get the output of the gyro for use with PIDControllers. May be the angle or
-   * rate depending on
-   * the set PIDSourceType
-   *
-   * @return the output according to the gyro
-   */
-  @Override
-  public double pidGet() {
-    switch (m_pidSource) {
-      case kRate:
-        return getRate();
-      case kDisplacement:
-        return getAngle();
-      default:
-        return 0.0;
-    }
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("Gyro");
-    builder.addDoubleProperty("Value", this::getAngle, null);
-  }
-}

src/main/java/frc4388/utility/RobotLogger.java

@@ -1,51 +0,0 @@
-package frc4388.utility;
-
-import java.util.ArrayList;
-import java.util.List;
-import java.util.Objects;
-import java.util.Optional;
-
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc4388.utility.PathPlannerUtil.Path.Waypoint;
-
-public final class RobotLogger {
-  private static RobotLogger instance = null;
-
-  public static RobotLogger getInstance() {
-    return Objects.requireNonNullElseGet(instance, () -> instance = new RobotLogger());
-  }
-
-  public static double getTime() {
-    return DriverStation.getMatchTime();
-  }
-
-  private RobotLogger() {
-    sendableChooser.setDefaultOption("Disable", false);
-    sendableChooser.addOption("Enable", true);
-    SmartDashboard.putData("Recording", sendableChooser);
-  }
-
-  private final List<Waypoint> data = new ArrayList<>();
-  private final SendableChooser<Boolean> sendableChooser = new SendableChooser<>();
-
-  double lastVelocityMetersPerSecond = 0;
-  public void put(Double anchorPointX, Double anchorPointY, Double prevControlX, Double prevControlY, Double nextControlX, Double nextControlY, Double holonomicAngle, Boolean isReversal, Double velOverride, Boolean isLocked) {
-    if (Boolean.TRUE.equals(sendableChooser.getSelected())) {
-      Waypoint waypoint = new Waypoint(anchorPointX, anchorPointY, prevControlX, prevControlY, nextControlX, nextControlY, holonomicAngle, isReversal, velOverride, isLocked);
-      data.add(waypoint);
-    }
-  }
-
-  @SuppressWarnings("unchecked")
-  public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
-    PathPlannerUtil.Path path = new PathPlannerUtil.Path();
-    path.waypoints = data.stream().map(Optional::ofNullable).toArray(Optional[]::new);
-    path.maxVelocity = Optional.ofNullable(maxVelocity);
-    path.maxAcceleration = Optional.ofNullable(maxAcceleration);
-    path.isReversed = Optional.ofNullable(isReversed);
-    data.clear();
-    return path;
-  }
-}

src/main/java/frc4388/utility/SendableRunChooser.java

@@ -0,0 +1,182 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+// package edu.wpi.first.wpilibj.smartdashboard;
+package frc4388.utility;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.ArrayList;
+import java.util.LinkedHashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Consumer;
+
+/**
+ * The {@link SendableRunChooser} class is a useful tool for presenting a selection of options to the
+ * {@link SmartDashboard}.
+ *
+ * <p>For instance, you may wish to be able to select between multiple autonomous modes. You can do
+ * this by putting every possible Command you want to run as an autonomous into a {@link
+ * SendableRunChooser} and then put it into the {@link SmartDashboard} to have a list of options appear
+ * on the laptop. Once autonomous starts, simply ask the {@link SendableRunChooser} what the selected
+ * value is.
+ *
+ * @param <V> The type of the values to be stored
+ */
+public class SendableRunChooser<V> implements NTSendable, AutoCloseable {
+  /** The key for the default value. */
+  private static final String DEFAULT = "default";
+  /** The key for the selected option. */
+  private static final String SELECTED = "selected";
+  /** The key for the active option. */
+  private static final String ACTIVE = "active";
+  /** The key for the option array. */
+  private static final String OPTIONS = "options";
+  /** The key for the instance number. */
+  private static final String INSTANCE = ".instance";
+  /** A map linking strings to the objects the represent. */
+  private final Map<String, V> m_map = new LinkedHashMap<>();
+
+  private String m_defaultChoice = "";
+  private final int m_instance;
+  private static final AtomicInteger s_instances = new AtomicInteger();
+
+  private Consumer<String> m_consumer;
+
+  /** Instantiates a {@link SendableRunChooser}. */
+  public SendableRunChooser(Consumer<String> consumer) {
+    m_consumer = consumer;
+    m_instance = s_instances.getAndIncrement();
+    SendableRegistry.add(this, "SendableChooser", m_instance);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the
+   * object will appear as the given name.
+   *
+   * @param name the name of the option
+   * @param object the option
+   */
+  public void addOption(String name, V object) {
+    m_map.put(name, object);
+  }
+
+  /**
+   * Adds the given object to the list of options.
+   *
+   * @param name the name of the option
+   * @param object the option
+   * @deprecated Use {@link #addOption(String, Object)} instead.
+   */
+  @Deprecated
+  public void addObject(String name, V object) {
+    addOption(name, object);
+  }
+
+  /**
+   * Adds the given object to the list of options and marks it as the default. Functionally, this is
+   * very close to {@link #addOption(String, Object)} except that it will use this as the default
+   * option if none other is explicitly selected.
+   *
+   * @param name the name of the option
+   * @param object the option
+   */
+  public void setDefaultOption(String name, V object) {
+    requireNonNullParam(name, "name", "setDefaultOption");
+
+    m_defaultChoice = name;
+    addOption(name, object);
+  }
+
+  /**
+   * Adds the given object to the list of options and marks it as the default.
+   *
+   * @param name the name of the option
+   * @param object the option
+   * @deprecated Use {@link #setDefaultOption(String, Object)} instead.
+   */
+  @Deprecated
+  public void addDefault(String name, V object) {
+    setDefaultOption(name, object);
+  }
+
+  /**
+   * Returns the selected option. If there is none selected, it will return the default. If there is
+   * none selected and no default, then it will return {@code null}.
+   *
+   * @return the option selected
+   */
+  public V getSelected() {
+    m_mutex.lock();
+    try {
+      if (m_selected != null) {
+        return m_map.get(m_selected);
+      } else {
+        return m_map.get(m_defaultChoice);
+      }
+    } finally {
+      m_mutex.unlock();
+    }
+  }
+
+  private String m_selected;
+  private final List<NetworkTableEntry> m_activeEntries = new ArrayList<>();
+  private final ReentrantLock m_mutex = new ReentrantLock();
+
+  @Override
+  public void initSendable(NTSendableBuilder builder) {
+    builder.setSmartDashboardType("String Chooser");
+    builder.getEntry(INSTANCE).setDouble(m_instance);
+    builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null);
+    builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null);
+    builder.addStringProperty(
+        ACTIVE,
+        () -> {
+          m_mutex.lock();
+          try {
+            if (m_selected != null) {
+              return m_selected;
+            } else {
+              return m_defaultChoice;
+            }
+          } finally {
+            m_mutex.unlock();
+          }
+        },
+        null);
+    m_mutex.lock();
+    try {
+      m_activeEntries.add(builder.getEntry(ACTIVE));
+    } finally {
+      m_mutex.unlock();
+    }
+    builder.addStringProperty(
+        SELECTED,
+        null,
+        val -> {
+          m_mutex.lock();
+          try {
+            m_selected = val;
+            m_consumer.accept(val);
+            for (NetworkTableEntry entry : m_activeEntries) {
+              entry.setString(val);
+            }
+          } finally {
+            m_mutex.unlock();
+          }
+        });
+  }
+}