Add compute test and properly initialize internal state

ros-controls · Nov 22, 2024 · a617a42 · a617a42
1 parent e01554a
commit a617a42
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 2 deletions.
diff --git a/include/control_filters/rate_limiter.hpp b/include/control_filters/rate_limiter.hpp
@@ -17,6 +17,7 @@
 
 #include <Eigen/Dense>
 #include <cmath>
+#include <limits>
 #include <memory>
 #include <string>
 
@@ -77,7 +78,7 @@ bool RateLimiter<T>::configure()
   logger_.reset(
     new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_)));
 
-  v0 = v1 = static_cast<T>(0.0);
+  v0 = v1 = std::numeric_limits<T>::quiet_NaN();
 
   // Initialize the parameters once
   if (!parameter_handler_)
@@ -130,6 +131,11 @@ bool RateLimiter<T>::update(const T & data_in, T & data_out)
     );
   }
   T v = data_in;
+  if (std::isnan(v0))
+  {
+    // not initialized yet
+    v1 = v0 = v;
+  }
   limiter->limit(v, v0, v1, static_cast<T>(parameters_.sampling_interval));
   // shift the values for the next update call
   v1 = v0;

diff --git a/test/control_filters/test_rate_limiter.cpp b/test/control_filters/test_rate_limiter.cpp
@@ -73,8 +73,22 @@ TEST_F(RateLimiterTest, TestRateLimiterCompute)
     ASSERT_TRUE(filter_->configure("", "TestRateLimiter",
         node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
 
-    double in, out;
+    double in = 10.0, out;
+    for (int i = 0; i < 10; i++)
+    {
+        ASSERT_NO_THROW(filter_->update(in, out));
+        // no change
+        EXPECT_THAT(out, ::testing::DoubleEq(in));
+    }
+    in = 0.0;
+    // takes 12 steps to reach 0 (first and second derivative limits)
+    for (int i = 0; i < 11; i++)
+    {
+        ASSERT_NO_THROW(filter_->update(in, out));
+        EXPECT_THAT(out, ::testing::Not(::testing::DoubleNear(in, 1e-6))) << "i=" << i;
+    }
     ASSERT_NO_THROW(filter_->update(in, out));
+    EXPECT_THAT(out, ::testing::DoubleNear(in, 1e-6));
 }
 
 int main(int argc, char ** argv)

diff --git a/test/control_filters/test_rate_limiter_parameters.yaml b/test/control_filters/test_rate_limiter_parameters.yaml
@@ -15,3 +15,11 @@ TestRateLimiterThrowsUnconfigured:
 TestRateLimiterCompute:
   ros__parameters:
     sampling_interval: 1.0
+    max_value: .NAN
+    min_value: .NAN
+    max_first_derivative_pos: 1.0
+    min_first_derivative_pos: -1.0
+    max_first_derivative_neg: 1.0
+    min_first_derivative_neg: -1.0
+    max_second_derivative: 0.1
+    min_second_derivative: -0.1