From 840b0e602ad42f6d356384e3c1cb4656877be494 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sun, 13 Oct 2024 23:17:13 +0000
Subject: [PATCH] More tests

---
 test/speed_limiter.cpp | 84 ++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 80 insertions(+), 4 deletions(-)

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);