diff --git a/test/speed_limiter.cpp b/test/speed_limiter.cpp index b3fa0b2a..887df9a5 100644 --- a/test/speed_limiter.cpp +++ b/test/speed_limiter.cpp @@ -17,7 +17,22 @@ #include "control_toolbox/speed_limiter.hpp" -TEST(SpeedLimiterTest, testLinearVelocityLimits) +TEST(SpeedLimiterTest, testNoLimits) +{ + control_toolbox::SpeedLimiter limiter; + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); +} + +TEST(SpeedLimiterTest, testVelocityLimits) { control_toolbox::SpeedLimiter limiter(true, true, true, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); @@ -44,13 +59,74 @@ TEST(SpeedLimiterTest, testLinearVelocityLimits) v = -10.0; limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); // acceleration is now limiting, not velocity - // check if the robot speed is now -0.25 m.s-1, which is 0.5m.s-2 * 0.5s + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + } +} + +TEST(SpeedLimiterTest, testVelocityNoLimits) +{ + { + control_toolbox::SpeedLimiter limiter(false, true, true, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit_velocity(v); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + v = -10.0; + limiting_factor = limiter.limit_velocity(v); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + } + + { + control_toolbox::SpeedLimiter limiter(false, true, true, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, 0.5); + EXPECT_DOUBLE_EQ(limiting_factor, 0.5/10.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // acceleration is now limiting, not velocity + // check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s + EXPECT_DOUBLE_EQ(v, -0.25); + EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); + } + + { + control_toolbox::SpeedLimiter limiter(false, false, true, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // jerk is now limiting, not velocity + EXPECT_DOUBLE_EQ(v, 2.5); + EXPECT_DOUBLE_EQ(limiting_factor, 2.5/10.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // jerk is now limiting, not velocity EXPECT_DOUBLE_EQ(v, -0.25); EXPECT_DOUBLE_EQ(limiting_factor, 0.25/10.0); } + + { + control_toolbox::SpeedLimiter limiter(false, false, false, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); + double v = 10.0; + double limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, 10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + v = -10.0; + limiting_factor = limiter.limit(v, 0.0, 0.0, 0.5); + // check if the velocity is not limited + EXPECT_DOUBLE_EQ(v, -10.0); + EXPECT_DOUBLE_EQ(limiting_factor, 1.0); + } } -TEST(SpeedLimiterTest, testLinearAccelerationLimits) +TEST(SpeedLimiterTest, testAccelerationLimits) { control_toolbox::SpeedLimiter limiter(true, true, true, -0.5, 1.0, -0.5, 1.0, -0.5, 5.0); @@ -83,7 +159,7 @@ TEST(SpeedLimiterTest, testLinearAccelerationLimits) } } -TEST(SpeedLimiterTest, testLinearJerkLimits) +TEST(SpeedLimiterTest, testJerkLimits) { control_toolbox::SpeedLimiter limiter(true, true, true, -0.5, 1.0, -0.5, 1.0, -1.0, 1.0);