From 95b6cd2dd9b5dd27c48790f65a390eb62c0cb40d Mon Sep 17 00:00:00 2001
From: Peter Johnson <johnson.peter@gmail.com>
Date: Sun, 5 Jan 2020 16:01:31 -0800
Subject: [PATCH] TrajectoryGenerator: Allow replacement of error reporting
 function (#2234)

This is needed to avoid use of DriverStation if used from desktop applications
such as PathWeaver.
---
 .../trajectory/TrajectoryGenerator.java       | 23 +++++++++++++++++--
 1 file changed, 21 insertions(+), 2 deletions(-)

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
index 2e2ae7dc65..a036de100e 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
@@ -11,6 +11,7 @@ import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Collection;
 import java.util.List;
+import java.util.function.BiConsumer;
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
@@ -26,6 +27,7 @@ import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException
 public final class TrajectoryGenerator {
   private static final Trajectory kDoNothingTrajectory =
       new Trajectory(Arrays.asList(new Trajectory.State()));
+  private static BiConsumer<String, StackTraceElement[]> errorFunc;
 
   /**
    * Private constructor because this is a utility class.
@@ -33,6 +35,23 @@ public final class TrajectoryGenerator {
   private TrajectoryGenerator() {
   }
 
+  private static void reportError(String error, StackTraceElement[] stackTrace) {
+    if (errorFunc != null) {
+      errorFunc.accept(error, stackTrace);
+    } else {
+      DriverStation.reportError(error, stackTrace);
+    }
+  }
+
+  /**
+   * Set error reporting function. By default, DriverStation.reportError() is used.
+   *
+   * @param func Error reporting function, arguments are error and stackTrace.
+   */
+  public static void setErrorHandler(BiConsumer<String, StackTraceElement[]> func) {
+    errorFunc = func;
+  }
+
   /**
    * Generates a trajectory from the given control vectors and config. This method uses clamped
    * cubic splines -- a method in which the exterior control vectors and interior waypoints
@@ -71,7 +90,7 @@ public final class TrajectoryGenerator {
       points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial,
           interiorWaypoints.toArray(new Translation2d[0]), newEnd));
     } catch (MalformedSplineException ex) {
-      DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
+      reportError(ex.getMessage(), ex.getStackTrace());
       return kDoNothingTrajectory;
     }
 
@@ -147,7 +166,7 @@ public final class TrajectoryGenerator {
           newControlVectors.toArray(new Spline.ControlVector[]{})
       ));
     } catch (MalformedSplineException ex) {
-      DriverStation.reportError(ex.getMessage(), ex.getStackTrace());
+      reportError(ex.getMessage(), ex.getStackTrace());
       return kDoNothingTrajectory;
     }