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 #include +#include #include #include @@ -77,7 +78,7 @@ bool RateLimiter::configure() logger_.reset( new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); - v0 = v1 = static_cast(0.0); + v0 = v1 = std::numeric_limits::quiet_NaN(); // Initialize the parameters once if (!parameter_handler_) @@ -130,6 +131,11 @@ bool RateLimiter::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(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