diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp index 92c21f630a..a9686337f7 100644 --- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp +++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp @@ -9,11 +9,16 @@ #include +#include "frc/DriverStation.h" #include "frc/spline/SplineHelper.h" +#include "frc/spline/SplineParameterizer.h" #include "frc/trajectory/TrajectoryParameterizer.h" using namespace frc; +const Trajectory TrajectoryGenerator::kDoNothingTrajectory( + std::vector{Trajectory::State()}); + Trajectory TrajectoryGenerator::GenerateTrajectory( Spline<3>::ControlVector initial, const std::vector& interiorWaypoints, @@ -29,9 +34,15 @@ Trajectory TrajectoryGenerator::GenerateTrajectory( end.y[1] *= -1; } - auto points = - SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors( - initial, interiorWaypoints, end)); + std::vector points; + try { + points = + SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors( + initial, interiorWaypoints, end)); + } catch (SplineParameterizer::MalformedSplineException& e) { + DriverStation::ReportError(e.what()); + return kDoNothingTrajectory; + } // After trajectory generation, flip theta back so it's relative to the // field. Also fix curvature. @@ -68,8 +79,14 @@ Trajectory TrajectoryGenerator::GenerateTrajectory( } } - auto points = SplinePointsFromSplines( - SplineHelper::QuinticSplinesFromControlVectors(controlVectors)); + std::vector points; + try { + points = SplinePointsFromSplines( + SplineHelper::QuinticSplinesFromControlVectors(controlVectors)); + } catch (SplineParameterizer::MalformedSplineException& e) { + DriverStation::ReportError(e.what()); + return kDoNothingTrajectory; + } // After trajectory generation, flip theta back so it's relative to the // field. Also fix curvature. diff --git a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h index 34cf026279..ecdede0e12 100644 --- a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h +++ b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h @@ -33,10 +33,13 @@ #include +#include +#include #include #include #include +#include namespace frc { @@ -47,6 +50,11 @@ class SplineParameterizer { public: using PoseWithCurvature = std::pair; + struct MalformedSplineException : public std::runtime_error { + explicit MalformedSplineException(const char* what_arg) + : runtime_error(what_arg) {} + }; + /** * Parameterizes the spline. This method breaks up the spline into various * arcs until their dx, dy, and dtheta are within specific tolerances. @@ -64,14 +72,48 @@ class SplineParameterizer { static std::vector Parameterize(const Spline& spline, double t0 = 0.0, double t1 = 1.0) { - std::vector arr; + std::vector splinePoints; - // The parameterization does not add the first initial point. Let's add - // that. - arr.push_back(spline.GetPoint(t0)); + // The parameterization does not add the initial point. Let's add that. + splinePoints.push_back(spline.GetPoint(t0)); - GetSegmentArc(spline, &arr, t0, t1); - return arr; + // We use an "explicit stack" to simulate recursion, instead of a recursive + // function call This give us greater control, instead of a stack overflow + std::stack stack; + stack.emplace(StackContents{t0, t1}); + + StackContents current; + PoseWithCurvature start; + PoseWithCurvature end; + int iterations = 0; + + while (!stack.empty()) { + current = stack.top(); + stack.pop(); + start = spline.GetPoint(current.t0); + end = spline.GetPoint(current.t1); + + const auto twist = start.first.Log(end.first); + + if (units::math::abs(twist.dy) > kMaxDy || + units::math::abs(twist.dx) > kMaxDx || + units::math::abs(twist.dtheta) > kMaxDtheta) { + stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1}); + stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2}); + } else { + splinePoints.push_back(spline.GetPoint(current.t1)); + } + + if (iterations++ >= kMaxIterations) { + throw MalformedSplineException( + "Could not parameterize a malformed spline. " + "This means that you probably had two or more adjacent " + "waypoints that were very close together with headings " + "in opposing directions."); + } + } + + return splinePoints; } private: @@ -80,33 +122,19 @@ class SplineParameterizer { static constexpr units::meter_t kMaxDy = 0.05_in; static constexpr units::radian_t kMaxDtheta = 0.0872_rad; + struct StackContents { + double t0; + double t1; + }; + /** - * Breaks up the spline into arcs until the dx, dy, and theta of each arc is - * within tolerance. - * - * @param spline The spline to parameterize. - * @param vector Pointer to vector of poses. - * @param t0 Starting point for arc. - * @param t1 Ending point for arc. + * A malformed spline does not actually explode the LIFO stack size. Instead, + * the stack size stays at a relatively small number (e.g. 30) and never + * decreases. Because of this, we must count iterations. Even long, complex + * paths don't usually go over 300 iterations, so hitting this maximum should + * definitely indicate something has gone wrong. */ - template - static void GetSegmentArc(const Spline& spline, - std::vector* vector, double t0, - double t1) { - const auto start = spline.GetPoint(t0); - const auto end = spline.GetPoint(t1); - - const auto twist = start.first.Log(end.first); - - if (units::math::abs(twist.dy) > kMaxDy || - units::math::abs(twist.dx) > kMaxDx || - units::math::abs(twist.dtheta) > kMaxDtheta) { - GetSegmentArc(spline, vector, t0, (t0 + t1) / 2); - GetSegmentArc(spline, vector, (t0 + t1) / 2, t1); - } else { - vector->push_back(spline.GetPoint(t1)); - } - } + static constexpr int kMaxIterations = 5000; friend class CubicHermiteSplineTest; friend class QuinticHermiteSplineTest; diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h index 4d6eff600c..55ef7829b0 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h +++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h @@ -113,5 +113,8 @@ class TrajectoryGenerator { } return splinePoints; } + + private: + static const Trajectory kDoNothingTrajectory; }; } // namespace frc diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp index 319be79d0b..63ecf2bc9c 100644 --- a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp +++ b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp @@ -120,3 +120,14 @@ TEST_F(CubicHermiteSplineTest, OneInterior) { Pose2d end{4_m, 0_m, 0_rad}; Run(start, waypoints, end); } + +TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) { + EXPECT_THROW( + Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector{}, + Pose2d{1_m, 0_m, Rotation2d(180_deg)}), + SplineParameterizer::MalformedSplineException); + EXPECT_THROW( + Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector{}, + Pose2d{10_m, 11_m, Rotation2d(-90_deg)}), + SplineParameterizer::MalformedSplineException); +} diff --git a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp index cc10c6c122..706ad20c53 100644 --- a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp +++ b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp @@ -85,3 +85,12 @@ TEST_F(QuinticHermiteSplineTest, SquigglyCurve) { Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)), Pose2d(-1_m, 0_m, Rotation2d(90_deg))); } + +TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) { + EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)), + Pose2d(1_m, 0_m, Rotation2d(180_deg))), + SplineParameterizer::MalformedSplineException); + EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)), + Pose2d(10_m, 11_m, Rotation2d(-90_deg))), + SplineParameterizer::MalformedSplineException); +} diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp index 7e47e112a7..c524281679 100644 --- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp +++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp @@ -33,3 +33,13 @@ TEST(TrajectoryGenerationTest, ObeysConstraints) { 12_fps_sq + 0.01_fps_sq); } } + +TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) { + const auto t = TrajectoryGenerator::GenerateTrajectory( + std::vector{Pose2d(0_m, 0_m, Rotation2d(0_deg)), + Pose2d(1_m, 0_m, Rotation2d(180_deg))}, + TrajectoryConfig(12_fps, 12_fps_sq)); + + ASSERT_EQ(t.States().size(), 1u); + ASSERT_EQ(t.TotalTime(), 0_s); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java index b78227d64c..1f141d05df 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java @@ -31,6 +31,7 @@ package edu.wpi.first.wpilibj.spline; +import java.util.ArrayDeque; import java.util.ArrayList; import java.util.List; @@ -42,6 +43,36 @@ public final class SplineParameterizer { private static final double kMaxDy = 0.00127; private static final double kMaxDtheta = 0.0872; + /** + * A malformed spline does not actually explode the LIFO stack size. Instead, the stack size + * stays at a relatively small number (e.g. 30) and never decreases. Because of this, we must + * count iterations. Even long, complex paths don't usually go over 300 iterations, so hitting + * this maximum should definitely indicate something has gone wrong. + */ + private static final int kMaxIterations = 5000; + + @SuppressWarnings("MemberName") + private static class StackContents { + final double t1; + final double t0; + + StackContents(double t0, double t1) { + this.t0 = t0; + this.t1 = t1; + } + } + + public static class MalformedSplineException extends RuntimeException { + /** + * Create a new exception with the given message. + * + * @param message the message to pass with the exception + */ + private MalformedSplineException(String message) { + super(message); + } + } + /** * Private constructor because this is a utility class. */ @@ -53,7 +84,9 @@ public final class SplineParameterizer { * arcs until their dx, dy, and dtheta are within specific tolerances. * * @param spline The spline to parameterize. - * @return A vector of poses and curvatures that represents various points on the spline. + * @return A list of poses and curvatures that represents various points on the spline. + * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points + * with approximately opposing headings) */ public static List parameterize(Spline spline) { return parameterize(spline, 0.0, 1.0); @@ -66,41 +99,54 @@ public final class SplineParameterizer { * @param spline The spline to parameterize. * @param t0 Starting internal spline parameter. It is recommended to use 0.0. * @param t1 Ending internal spline parameter. It is recommended to use 1.0. - * @return A vector of poses and curvatures that represents various points on the spline. + * @return A list of poses and curvatures that represents various points on the spline. + * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points + * with approximately opposing headings) */ + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") public static List parameterize(Spline spline, double t0, double t1) { - var arr = new ArrayList(); + var splinePoints = new ArrayList(); - // The parameterization does not add the first initial point. Let's add - // that. - arr.add(spline.getPoint(t0)); + // The parameterization does not add the initial point. Let's add that. + splinePoints.add(spline.getPoint(t0)); - getSegmentArc(spline, arr, t0, t1); - return arr; - } + // We use an "explicit stack" to simulate recursion, instead of a recursive function call + // This give us greater control, instead of a stack overflow + var stack = new ArrayDeque(); + stack.push(new StackContents(t0, t1)); - /** - * Breaks up the spline into arcs until the dx, dy, and theta of each arc is - * within tolerance. - * - * @param spline The spline to parameterize. - * @param vector Pointer to vector of poses. - * @param t0 Starting point for arc. - * @param t1 Ending point for arc. - */ - private static void getSegmentArc(Spline spline, List vector, - double t0, double t1) { - final var start = spline.getPoint(t0); - final var end = spline.getPoint(t1); + StackContents current; + PoseWithCurvature start; + PoseWithCurvature end; + int iterations = 0; - final var twist = start.poseMeters.log(end.poseMeters); + while (!stack.isEmpty()) { + current = stack.removeFirst(); + start = spline.getPoint(current.t0); + end = spline.getPoint(current.t1); - if (Math.abs(twist.dy) > kMaxDy || Math.abs(twist.dx) > kMaxDx - || Math.abs(twist.dtheta) > kMaxDtheta) { - getSegmentArc(spline, vector, t0, (t0 + t1) / 2); - getSegmentArc(spline, vector, (t0 + t1) / 2, t1); - } else { - vector.add(spline.getPoint(t1)); + final var twist = start.poseMeters.log(end.poseMeters); + if ( + Math.abs(twist.dy) > kMaxDy + || Math.abs(twist.dx) > kMaxDx + || Math.abs(twist.dtheta) > kMaxDtheta + ) { + stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1)); + stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2)); + } else { + splinePoints.add(spline.getPoint(current.t1)); + } + + iterations++; + if (iterations >= kMaxIterations) { + throw new MalformedSplineException( + "Could not parameterize a malformed spline. " + + "This means that you probably had two or more adjacent waypoints that were very close " + + "together with headings in opposing directions." + ); + } } + + return splinePoints; } } 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 a4d6898597..6d3a89f771 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 @@ -8,9 +8,11 @@ package edu.wpi.first.wpilibj.trajectory; import java.util.ArrayList; +import java.util.Arrays; import java.util.Collection; import java.util.List; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Transform2d; @@ -19,8 +21,12 @@ import edu.wpi.first.wpilibj.spline.PoseWithCurvature; import edu.wpi.first.wpilibj.spline.Spline; import edu.wpi.first.wpilibj.spline.SplineHelper; import edu.wpi.first.wpilibj.spline.SplineParameterizer; +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 constructor because this is a utility class. */ @@ -60,9 +66,14 @@ public final class TrajectoryGenerator { } // Get the spline points - var points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors( - newInitial, interiorWaypoints.toArray(new Translation2d[0]), newEnd - )); + List points; + try { + points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial, + interiorWaypoints.toArray(new Translation2d[0]), newEnd)); + } catch (MalformedSplineException ex) { + DriverStation.reportError(ex.getMessage(), ex.getStackTrace()); + return kDoNothingTrajectory; + } // Change the points back to their original orientation. if (config.isReversed()) { @@ -130,9 +141,15 @@ public final class TrajectoryGenerator { } // Get the spline points - var points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors( - newControlVectors.toArray(new Spline.ControlVector[]{}) - )); + List points; + try { + points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors( + newControlVectors.toArray(new Spline.ControlVector[]{}) + )); + } catch (MalformedSplineException ex) { + DriverStation.reportError(ex.getMessage(), ex.getStackTrace()); + return kDoNothingTrajectory; + } // Change the points back to their original orientation. if (config.isReversed()) { @@ -171,6 +188,8 @@ public final class TrajectoryGenerator { * * @param splines The splines to parameterize. * @return The spline points for use in time parameterization of a trajectory. + * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points + * with approximately opposing headings) */ public static List splinePointsFromSplines( Spline[] splines) { diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java index 5a8349d6d3..e656f81493 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java @@ -8,6 +8,7 @@ package edu.wpi.first.wpilibj.spline; import java.util.ArrayList; +import java.util.Arrays; import java.util.List; import org.junit.jupiter.api.Test; @@ -15,9 +16,11 @@ import org.junit.jupiter.api.Test; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -145,4 +148,15 @@ class CubicHermiteSplineTest { run(start, waypoints, end); } + + @Test + void testMalformed() { + assertThrows(MalformedSplineException.class, () -> run( + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180)))); + assertThrows(MalformedSplineException.class, () -> run( + new Pose2d(10, 10, Rotation2d.fromDegrees(90)), + Arrays.asList(new Translation2d(10, 10.5)), + new Pose2d(10, 11, Rotation2d.fromDegrees(-90)))); + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java index 5eb84374bb..0bd4f6934c 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java @@ -13,9 +13,11 @@ import org.junit.jupiter.api.Test; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; import static org.junit.jupiter.api.Assertions.assertTrue; class QuinticHermiteSplineTest { @@ -23,7 +25,7 @@ class QuinticHermiteSplineTest { private static final double kMaxDy = 0.00127; private static final double kMaxDtheta = 0.0872; - @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"}) + @SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" }) private void run(Pose2d a, Pose2d b) { // Start the timer. var start = System.nanoTime(); @@ -49,29 +51,27 @@ class QuinticHermiteSplineTest { assertAll( () -> assertTrue(Math.abs(twist.dx) < kMaxDx), () -> assertTrue(Math.abs(twist.dy) < kMaxDy), - () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta) - ); + () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta)); } // Check first point assertAll( - () -> assertEquals(a.getTranslation().getX(), - poses.get(0).poseMeters.getTranslation().getX(), 1E-9), - () -> assertEquals(a.getTranslation().getY(), - poses.get(0).poseMeters.getTranslation().getY(), 1E-9), - () -> assertEquals(a.getRotation().getRadians(), - poses.get(0).poseMeters.getRotation().getRadians(), 1E-9) - ); + () -> assertEquals( + a.getTranslation().getX(), poses.get(0).poseMeters.getTranslation().getX(), 1E-9), + () -> assertEquals( + a.getTranslation().getY(), poses.get(0).poseMeters.getTranslation().getY(), 1E-9), + () -> assertEquals( + a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(), + 1E-9)); // Check last point assertAll( - () -> assertEquals(b.getTranslation().getX(), - poses.get(poses.size() - 1).poseMeters.getTranslation().getX(), 1E-9), - () -> assertEquals(b.getTranslation().getY(), - poses.get(poses.size() - 1).poseMeters.getTranslation().getY(), 1E-9), + () -> assertEquals(b.getTranslation().getX(), poses.get(poses.size() - 1) + .poseMeters.getTranslation().getX(), 1E-9), + () -> assertEquals(b.getTranslation().getY(), poses.get(poses.size() - 1) + .poseMeters.getTranslation().getY(), 1E-9), () -> assertEquals(b.getRotation().getRadians(), - poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9) - ); + poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)); } @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") @@ -89,7 +89,20 @@ class QuinticHermiteSplineTest { @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") @Test void testSquiggly() { - run(new Pose2d(0, 0, Rotation2d.fromDegrees(90)), + run( + new Pose2d(0, 0, Rotation2d.fromDegrees(90)), new Pose2d(-1, 0, Rotation2d.fromDegrees(90))); } + + @Test + void testMalformed() { + assertThrows(MalformedSplineException.class, + () -> run( + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + new Pose2d(1, 0, Rotation2d.fromDegrees(180)))); + assertThrows(MalformedSplineException.class, + () -> run( + new Pose2d(10, 10, Rotation2d.fromDegrees(90)), + new Pose2d(10, 11, Rotation2d.fromDegrees(-90)))); + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java index 5b45e3e590..2cd309d9d5 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java @@ -8,10 +8,12 @@ package edu.wpi.first.wpilibj.trajectory; import java.util.ArrayList; +import java.util.Arrays; import java.util.List; import org.junit.jupiter.api.Test; +import edu.wpi.first.hal.sim.DriverStationSim; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Transform2d; @@ -20,6 +22,7 @@ import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint; import static edu.wpi.first.wpilibj.util.Units.feetToMeters; import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; class TrajectoryGeneratorTest { @@ -69,4 +72,24 @@ class TrajectoryGeneratorTest { ); } } + + @Test + void testMalformedTrajectory() { + var dsSim = new DriverStationSim(); + dsSim.setSendError(false); + + var traj = + TrajectoryGenerator.generateTrajectory( + Arrays.asList( + new Pose2d(0, 0, Rotation2d.fromDegrees(0)), + new Pose2d(1, 0, Rotation2d.fromDegrees(180)) + ), + new TrajectoryConfig(feetToMeters(12), feetToMeters(12)) + ); + + assertEquals(traj.getStates().size(), 1); + assertEquals(traj.getTotalTimeSeconds(), 0); + + dsSim.setSendError(true); + } }