[wpimath] fix null pointer when calling timeLeftUntill

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java

@@ -43,7 +43,7 @@ public class TrapezoidProfile {
   private int m_direction;
 
   private final Constraints m_constraints;
-  private State m_current;
+  private State m_current = new State();
 
   private double m_endAccel;
   private double m_endFullSpeed;
@@ -187,7 +187,8 @@ public State calculate(double t, State current, State goal) {
    * Returns the time left until a target distance in the profile is reached.
    *
    * @param target The target distance.
-   * @return The time left until a target distance in the profile is reached.
+   * @return The time left until a target distance in the profile is reached, or zero if no goal was
+   *     set.
    */
   public double timeLeftUntil(double target) {
     double position = m_current.position * m_direction;
@@ -253,7 +254,7 @@ public double timeLeftUntil(double target) {
   /**
    * Returns the total time the profile takes to reach the goal.
    *
-   * @return The total time the profile takes to reach the goal.
+   * @return The total time the profile takes to reach the goal, or zero if no goal was set.
    */
   public double totalTime() {
     return m_endDecel;

wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h

@@ -200,7 +200,8 @@ class TrapezoidProfile {
    * Returns the time left until a target distance in the profile is reached.
    *
    * @param target The target distance.
-   * @return The time left until a target distance in the profile is reached.
+   * @return The time left until a target distance in the profile is reached, or
+   * zero if no goal was set.
    */
   constexpr units::second_t TimeLeftUntil(Distance_t target) const {
     Distance_t position = m_current.position * m_direction;
@@ -271,7 +272,8 @@ class TrapezoidProfile {
   /**
    * Returns the total time the profile takes to reach the goal.
    *
-   * @return The total time the profile takes to reach the goal.
+   * @return The total time the profile takes to reach the goal, or zero if no
+   * goal was set.
    */
   constexpr units::second_t TotalTime() const { return m_endDecel; }
 
@@ -311,14 +313,14 @@ class TrapezoidProfile {
   }
 
   // The direction of the profile, either 1 for forwards or -1 for inverted
-  int m_direction;
+  int m_direction = 1;
 
   Constraints m_constraints;
   State m_current;
 
-  units::second_t m_endAccel;
-  units::second_t m_endFullSpeed;
-  units::second_t m_endDecel;
+  units::second_t m_endAccel = 0_s;
+  units::second_t m_endFullSpeed = 0_s;
+  units::second_t m_endDecel = 0_s;
 };
 
 }  // namespace frc

wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java

@@ -246,4 +246,11 @@ void timingBeforeNegativeGoal() {
       }
     }
   }
+
+  @Test
+  void initalizationOfCurrentState() {
+    var profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(1, 1));
+    assertNear(profile.timeLeftUntil(0), 0, 1e-10);
+    assertNear(profile.totalTime(), 0, 1e-10);
+  }
 }

wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp

@@ -234,3 +234,10 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
     }
   }
 }
+
+TEST(TrapezoidProfileTest, InitalizationOfCurrentState) {
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{1_mps, 1_mps_sq};
+  frc::TrapezoidProfile<units::meter> profile{constraints};
+  EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s);
+  EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s);
+}