[wpimath] Move WPILib math classes into wpimath

This includes LinearFilter and MedianFilter, controller feedforwards,
geometry, kinematics, spline, and trajectory classes.

Package names and include directories were not changed to avoid code breakage.

.styleguide

@@ -20,17 +20,20 @@ repoRootNameOverride {
 }
 
 includeOtherLibs {
+  ^Eigen/
   ^cameraserver/
   ^cscore
   ^drake/
   ^hal/
   ^imgui
+  ^math/
   ^mockdata/
   ^networktables/
   ^ntcore
   ^opencv2/
   ^support/
   ^units/
+  ^unsupported/
   ^vision/
   ^wpi/
 }

wpilibc/src/main/native/cppcs/RobotBase.cpp

@@ -16,6 +16,7 @@
 #include <cameraserver/CameraServerShared.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
+#include <math/MathShared.h>
 #include <networktables/NetworkTableInstance.h>
 
 #include "WPILibVersion.h"
@@ -25,7 +26,6 @@
 #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);
 
@@ -69,6 +69,47 @@ class WPILibCameraServerShared : public frc::CameraServerShared {
     return std::make_pair(RobotBase::GetThreadId(), true);
   }
 };
+class WPILibMathShared : public wpi::math::MathShared {
+ public:
+  void ReportError(const wpi::Twine& error) override {
+    DriverStation::ReportError(error);
+  }
+
+  void ReportUsage(wpi::math::MathUsageId id, int count) override {
+    switch (id) {
+      case wpi::math::MathUsageId::kKinematics_DifferentialDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_DifferentialDrive);
+        break;
+      case wpi::math::MathUsageId::kKinematics_MecanumDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_MecanumDrive);
+        break;
+      case wpi::math::MathUsageId::kKinematics_SwerveDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_SwerveDrive);
+        break;
+      case wpi::math::MathUsageId::kTrajectory_TrapezoidProfile:
+        HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, count);
+        break;
+      case wpi::math::MathUsageId::kFilter_Linear:
+        HAL_Report(HALUsageReporting::kResourceType_LinearFilter, count);
+        break;
+      case wpi::math::MathUsageId::kOdometry_DifferentialDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_DifferentialDrive);
+        break;
+      case wpi::math::MathUsageId::kOdometry_SwerveDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_SwerveDrive);
+        break;
+      case wpi::math::MathUsageId::kOdometry_MecanumDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_MecanumDrive);
+        break;
+    }
+  }
+};
 }  // namespace
 
 static void SetupCameraServerShared() {
@@ -102,6 +143,11 @@ static void SetupCameraServerShared() {
 #endif
 }
 
+static void SetupMathShared() {
+  wpi::math::MathSharedStore::SetMathShared(
+      std::make_unique<WPILibMathShared>());
+}
+
 bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
 
 bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
@@ -120,8 +166,7 @@ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
   m_threadId = std::this_thread::get_id();
 
   SetupCameraServerShared();
-  TrajectoryGenerator::SetErrorHandler(
-      [](const char* error) { DriverStation::ReportError(error); });
+  SetupMathShared();
 
   auto inst = nt::NetworkTableInstance::GetDefault();
   inst.SetNetworkIdentity("Robot");

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java

@@ -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.                                                               */
@@ -22,6 +22,9 @@
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.math.MathShared;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -78,6 +81,53 @@ public boolean isRoboRIO() {
     CameraServerSharedStore.setCameraServerShared(shared);
   }
 
+  private static void setupMathShared() {
+    MathSharedStore.setMathShared(new MathShared() {
+      @Override
+      public void reportError(String error, StackTraceElement[] stackTrace) {
+        DriverStation.reportError(error, stackTrace);
+      }
+
+      @Override
+      public void reportUsage(MathUsageId id, int count) {
+        switch (id) {
+          case kKinematics_DifferentialDrive:
+            HAL.report(tResourceType.kResourceType_Kinematics,
+                tInstances.kKinematics_DifferentialDrive);
+            break;
+          case kKinematics_MecanumDrive:
+            HAL.report(tResourceType.kResourceType_Kinematics,
+                tInstances.kKinematics_MecanumDrive);
+            break;
+          case kKinematics_SwerveDrive:
+            HAL.report(tResourceType.kResourceType_Kinematics,
+                tInstances.kKinematics_SwerveDrive);
+            break;
+          case kTrajectory_TrapezoidProfile:
+            HAL.report(tResourceType.kResourceType_TrapezoidProfile, count);
+            break;
+          case kFilter_Linear:
+            HAL.report(tResourceType.kResourceType_LinearFilter, count);
+            break;
+          case kOdometry_DifferentialDrive:
+            HAL.report(tResourceType.kResourceType_Odometry,
+                tInstances.kOdometry_DifferentialDrive);
+            break;
+          case kOdometry_SwerveDrive:
+            HAL.report(tResourceType.kResourceType_Odometry,
+                tInstances.kOdometry_SwerveDrive);
+            break;
+          case kOdometry_MecanumDrive:
+            HAL.report(tResourceType.kResourceType_Odometry,
+                tInstances.kOdometry_MecanumDrive);
+            break;
+          default:
+            break;
+        }
+      }
+    });
+  }
+
   protected final DriverStation m_ds;
 
   /**
@@ -90,9 +140,10 @@ public boolean isRoboRIO() {
    * to put this code into it's own task that loads on boot so ensure that it runs.
    */
   protected RobotBase() {
-    NetworkTableInstance inst = NetworkTableInstance.getDefault();
+    final NetworkTableInstance inst = NetworkTableInstance.getDefault();
     m_threadId = Thread.currentThread().getId();
     setupCameraServerShared();
+    setupMathShared();
     inst.setNetworkIdentity("Robot");
     if (isReal()) {
       inst.startServer("/home/lvuser/networktables.ini");

wpimath/.styleguide

@@ -31,5 +31,7 @@ includeGuardRoots {
 }
 
 includeOtherLibs {
+  ^Eigen/
+  ^unsupported/
   ^wpi/
 }

wpimath/CMakeLists.txt

@@ -38,8 +38,9 @@ if (NOT WITHOUT_JAVA)
   endif()
 
   file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
+  file(GLOB JACKSON_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
 
-  set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${EJML_JARS})
+  set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${EJML_JARS} ${JACKSON_JARS})
 
   execute_process(COMMAND python3 ${CMAKE_SOURCE_DIR}/wpimath/generate_numbers.py ${CMAKE_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
   if(NOT (generateResult EQUAL "0"))

wpimath/src/main/java/edu/wpi/first/math/MathShared.java

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+public interface MathShared {
+  /**
+   * Report an error.
+   *
+   * @param error the error to set
+   */
+  void reportError(String error, StackTraceElement[] stackTrace);
+
+  /**
+   * Report usage.
+   *
+   * @param id the usage id
+   * @param count the usage count
+   */
+  void reportUsage(MathUsageId id, int count);
+}

wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java

@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+public final class MathSharedStore {
+  private static MathShared mathShared;
+
+  private MathSharedStore() {
+  }
+
+  /**
+   * get the MathShared object.
+   */
+  public static synchronized MathShared getMathShared() {
+    if (mathShared == null) {
+      mathShared = new MathShared() {
+        @Override
+        public void reportError(String error, StackTraceElement[] stackTrace) {
+        }
+
+        @Override
+        public void reportUsage(MathUsageId id, int count) {
+        }
+      };
+    }
+    return mathShared;
+  }
+
+  /**
+   * set the MathShared object.
+   */
+  public static synchronized void setMathShared(MathShared shared) {
+    mathShared = shared;
+  }
+
+  /**
+   * Report an error.
+   *
+   * @param error the error to set
+   */
+  public static void reportError(String error, StackTraceElement[] stackTrace) {
+    getMathShared().reportError(error, stackTrace);
+  }
+
+  /**
+   * Report usage.
+   *
+   * @param id the usage id
+   * @param count the usage count
+   */
+  public static void reportUsage(MathUsageId id, int count) {
+    getMathShared().reportUsage(id, count);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java

@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+public enum MathUsageId {
+  kKinematics_DifferentialDrive,
+  kKinematics_MecanumDrive,
+  kKinematics_SwerveDrive,
+  kTrajectory_TrapezoidProfile,
+  kFilter_Linear,
+  kOdometry_DifferentialDrive,
+  kOdometry_SwerveDrive,
+  kOdometry_MecanumDrive
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java → wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-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.                                                               */
@@ -9,8 +9,8 @@
 
 import java.util.Arrays;
 
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.wpiutil.CircularBuffer;
 
 /**
@@ -71,7 +71,7 @@ public LinearFilter(double[] ffGains, double[] fbGains) {
     m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
 
     instances++;
-    HAL.report(tResourceType.kResourceType_LinearFilter, instances);
+    MathSharedStore.reportUsage(MathUsageId.kFilter_Linear, instances);
   }
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java → wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-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.                                                               */

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java → wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-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.                                                               */

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java → wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-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.                                                               */

wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java → wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-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.                                                               */

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java → wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-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.                                                               */

wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java → wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-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.                                                               */