From c6336cdd8935c56dea377e09487ef608923070a3 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Fri, 22 Nov 2024 20:53:24 +0000
Subject: [PATCH] Add compute test and properly initialize internal state

---
 include/control_filters/rate_limiter.hpp         |  8 +++++++-
 test/control_filters/test_rate_limiter.cpp       | 16 +++++++++++++++-
 .../test_rate_limiter_parameters.yaml            |  8 ++++++++
 3 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/include/control_filters/rate_limiter.hpp b/include/control_filters/rate_limiter.hpp
index d72d0b23..4be8f357 100644
--- 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
index c985efbf..d1f4e55c 100644
--- 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
index 6acb9d8a..b87f85d2 100644
--- 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