From dcb96cb50c8272716435f5e247bd07443bb780ac Mon Sep 17 00:00:00 2001
From: Peter Johnson <johnson.peter@gmail.com>
Date: Mon, 13 Jan 2020 20:36:16 -0800
Subject: [PATCH] TrajectoryGenerator: Allow replacement of error reporting
 function (C++) (#2267)

C++ version of #2234.
---
 .../native/cpp/trajectory/TrajectoryGenerator.cpp | 15 ++++++++++++---
 wpilibc/src/main/native/cppcs/RobotBase.cpp       |  5 ++++-
 .../include/frc/trajectory/TrajectoryGenerator.h  | 13 +++++++++++++
 3 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
index 3c95472efe..03821e2337 100644
--- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -9,7 +9,8 @@
 
 #include <utility>
 
-#include "frc/DriverStation.h"
+#include <wpi/raw_ostream.h>
+
 #include "frc/spline/SplineHelper.h"
 #include "frc/spline/SplineParameterizer.h"
 #include "frc/trajectory/TrajectoryParameterizer.h"
@@ -18,6 +19,14 @@ using namespace frc;
 
 const Trajectory TrajectoryGenerator::kDoNothingTrajectory(
     std::vector<Trajectory::State>{Trajectory::State()});
+std::function<void(const char*)> TrajectoryGenerator::s_errorFunc;
+
+void TrajectoryGenerator::ReportError(const char* error) {
+  if (s_errorFunc)
+    s_errorFunc(error);
+  else
+    wpi::errs() << "TrajectoryGenerator error: " << error << "\n";
+}
 
 Trajectory TrajectoryGenerator::GenerateTrajectory(
     Spline<3>::ControlVector initial,
@@ -40,7 +49,7 @@ Trajectory TrajectoryGenerator::GenerateTrajectory(
         SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
             initial, interiorWaypoints, end));
   } catch (SplineParameterizer::MalformedSplineException& e) {
-    DriverStation::ReportError(e.what());
+    ReportError(e.what());
     return kDoNothingTrajectory;
   }
 
@@ -84,7 +93,7 @@ Trajectory TrajectoryGenerator::GenerateTrajectory(
     points = SplinePointsFromSplines(
         SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
   } catch (SplineParameterizer::MalformedSplineException& e) {
-    DriverStation::ReportError(e.what());
+    ReportError(e.what());
     return kDoNothingTrajectory;
   }
 
diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp
index eaf4ddcdcf..54c42284bc 100644
--- a/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -25,6 +25,7 @@
 #include "frc/WPIErrors.h"
 #include "frc/livewindow/LiveWindow.h"
 #include "frc/smartdashboard/SmartDashboard.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
 
 typedef void (*SetCameraServerSharedFP)(frc::CameraServerShared* shared);
 
@@ -121,6 +122,8 @@ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
   m_threadId = std::this_thread::get_id();
 
   SetupCameraServerShared();
+  TrajectoryGenerator::SetErrorHandler(
+      [](const char* error) { DriverStation::ReportError(error); });
 
   auto inst = nt::NetworkTableInstance::GetDefault();
   inst.SetNetworkIdentity("Robot");
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
index 40e7e7b8d4..d90d4ba9a4 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <functional>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -114,7 +115,19 @@ class TrajectoryGenerator {
     return splinePoints;
   }
 
+  /**
+   * Set error reporting function. By default, it is output to stderr.
+   *
+   * @param func Error reporting function.
+   */
+  static void SetErrorHandler(std::function<void(const char*)> func) {
+    s_errorFunc = std::move(func);
+  }
+
  private:
+  static void ReportError(const char* error);
+
   static const Trajectory kDoNothingTrajectory;
+  static std::function<void(const char*)> s_errorFunc;
 };
 }  // namespace frc