More tests

ros-controls · Oct 13, 2024 · 840b0e6 · 840b0e6
1 parent fd15fc1
commit 840b0e6
Showing 1 changed file with 80 additions and 4 deletions.
diff --git 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);