From e28b3c9d72d3592bd8df15579d8a19be9baf81f4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 20 Nov 2024 21:47:25 +0000 Subject: [PATCH 1/8] Fix copyright claim --- test/control_filters/test_load_rate_limiter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/control_filters/test_load_rate_limiter.cpp b/test/control_filters/test_load_rate_limiter.cpp index 649e1983..b9a6e1ad 100644 --- a/test/control_filters/test_load_rate_limiter.cpp +++ b/test/control_filters/test_load_rate_limiter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright 2024 AIT - Austrian Institute of Technology GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From d47ac7d08236defcfc6292b3bff296037fa631df Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 20 Nov 2024 21:55:48 +0000 Subject: [PATCH 2/8] Add custom validator excluding NaN values and update parameter description --- include/control_filters/custom_validators.hpp | 58 +++++++++++++++++++ include/control_filters/rate_limiter.hpp | 1 + .../rate_limiter_parameters.yaml | 34 ++++++++++- 3 files changed, 92 insertions(+), 1 deletion(-) create mode 100644 include/control_filters/custom_validators.hpp diff --git a/include/control_filters/custom_validators.hpp b/include/control_filters/custom_validators.hpp new file mode 100644 index 00000000..e318bd2d --- /dev/null +++ b/include/control_filters/custom_validators.hpp @@ -0,0 +1,58 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_FILTERS__CUSTOM_VALIDATORS_HPP_ +#define CONTROL_FILTERS__CUSTOM_VALIDATORS_HPP_ + +#include + +#include + +#include +#include +#include + +namespace control_filters { + +/** + * @brief gt_eq, but check only if the value is not NaN + */ +template +tl::expected gt_eq_or_nan( + rclcpp::Parameter const& parameter, T expected_value) { + auto param_value = parameter.as_double(); + if (!std::isnan(param_value)) { + // check only if the value is not NaN + return rsl::gt_eq(parameter, expected_value); + } + return {}; +} + +/** + * @brief lt_eq, but check only if the value is not NaN + */ +template +tl::expected lt_eq_or_nan( + rclcpp::Parameter const& parameter, T expected_value) { + auto param_value = parameter.as_double(); + if (!std::isnan(param_value)) { + // check only if the value is not NaN + return rsl::lt_eq(parameter, expected_value); + } + return {}; +} + +} // namespace control_filters + +#endif // CONTROL_FILTERS__CUSTOM_VALIDATORS_HPP_ diff --git a/include/control_filters/rate_limiter.hpp b/include/control_filters/rate_limiter.hpp index 2aa22072..16a93fd7 100644 --- a/include/control_filters/rate_limiter.hpp +++ b/include/control_filters/rate_limiter.hpp @@ -23,6 +23,7 @@ #include "filters/filter_base.hpp" #include "control_toolbox/rate_limiter.hpp" +#include "custom_validators.hpp" #include "rate_limiter_parameters.hpp" namespace control_filters diff --git a/src/control_filters/rate_limiter_parameters.yaml b/src/control_filters/rate_limiter_parameters.yaml index ec9027b7..ea142b95 100644 --- a/src/control_filters/rate_limiter_parameters.yaml +++ b/src/control_filters/rate_limiter_parameters.yaml @@ -3,38 +3,70 @@ rate_limiter: type: double, description: "Sampling interval in seconds", validation: { - gt<>: [0.0] + gt<>: [0.0], }, } max_value: { type: double, default_value: .NAN, + description: "Maximum value, e.g. [m/s]", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + }, } min_value: { type: double, default_value: .NAN, + description: "Minimum value, e.g. [m/s]", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + }, } max_first_derivative_pos: { type: double, default_value: .NAN, + description: "Maximum value of the first derivative if **value** is positive, e.g. [m/s^2]", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + }, } min_first_derivative_pos: { type: double, default_value: .NAN, + description: "Minimum value of the first derivative if **value** is positive, e.g. [m/s^2]", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + }, } max_first_derivative_neg: { type: double, default_value: .NAN, + description: "Maximum value of the first derivative if **value** is negative, e.g. [m/s^2]", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + }, } min_first_derivative_neg: { type: double, default_value: .NAN, + description: "Minimum value of the first derivative if **value** is negative, e.g. [m/s^2]", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + }, } max_second_derivative: { type: double, default_value: .NAN, + description: "Maximum value of the second derivative, e.g. [m/s^3]", + validation: { + "control_filters::gt_eq_or_nan<>": [0.0] + }, } min_second_derivative: { type: double, default_value: .NAN, + description: "Minimum value of the second derivative, e.g. [m/s^3]", + validation: { + "control_filters::lt_eq_or_nan<>": [0.0] + }, } From d37ff0a7c8cc9429028f7f3877c9ce8ad7a3dd8b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 20 Nov 2024 22:04:31 +0000 Subject: [PATCH 3/8] Add tests --- CMakeLists.txt | 6 ++ test/control_filters/test_rate_limiter.cpp | 87 +++++++++++++++++++ test/control_filters/test_rate_limiter.hpp | 62 +++++++++++++ .../test_rate_limiter_parameters.yaml | 17 ++++ 4 files changed, 172 insertions(+) create mode 100644 test/control_filters/test_rate_limiter.cpp create mode 100644 test/control_filters/test_rate_limiter.hpp create mode 100644 test/control_filters/test_rate_limiter_parameters.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 4f144e88..8242a3ec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -133,6 +133,12 @@ if(BUILD_TESTING) target_link_libraries(test_load_low_pass_filter low_pass_filter low_pass_filter_parameters) ament_target_dependencies(test_load_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + add_rostest_with_parameters_gmock(test_rate_limiter test/control_filters/test_rate_limiter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_rate_limiter_parameters.yaml + ) + target_link_libraries(test_rate_limiter rate_limiter rate_limiter_parameters) + ament_target_dependencies(test_rate_limiter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + ament_add_gmock(test_load_rate_limiter test/control_filters/test_load_rate_limiter.cpp) target_link_libraries(test_load_rate_limiter rate_limiter rate_limiter_parameters) ament_target_dependencies(test_load_rate_limiter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) diff --git a/test/control_filters/test_rate_limiter.cpp b/test/control_filters/test_rate_limiter.cpp new file mode 100644 index 00000000..6ba29b60 --- /dev/null +++ b/test/control_filters/test_rate_limiter.cpp @@ -0,0 +1,87 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_rate_limiter.hpp" + +TEST_F(RateLimiterTest, TestRateLimiterAllParameters) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // should allow configuration and find parameters in sensor_filter_chain param namespace + ASSERT_TRUE(filter_->configure("", "TestRateLimiter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // change a parameter + node_->set_parameter(rclcpp::Parameter("sampling_interval", 0.5)); + // accept second call to configure with valid parameters to already configured filter + ASSERT_TRUE(filter_->configure("", "TestRateLimiter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} + + +TEST_F(RateLimiterTest, TestRateLimiterMissingParameter) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // should deny configuration as sampling frequency is missing + ASSERT_FALSE(filter_->configure("", "TestRateLimiter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} + +TEST_F(RateLimiterTest, TestRateLimiterInvalidThenFixedParameter) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // should deny configuration as sampling frequency is invalid + ASSERT_FALSE(filter_->configure("", "TestRateLimiter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // fix the param + node_->set_parameter(rclcpp::Parameter("sampling_interval", 1.0)); + // should allow configuration and pass second call to unconfigured filter + ASSERT_TRUE(filter_->configure("", "TestRateLimiter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} + +TEST_F(RateLimiterTest, TestRateLimiterThrowsUnconfigured) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + double in, out; + ASSERT_THROW(filter_->update(in, out), std::runtime_error); +} + +TEST_F(RateLimiterTest, TestRateLimiterCompute) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + ASSERT_TRUE(filter_->configure("", "TestRateLimiter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + double in, out; + ASSERT_NO_THROW(filter_->update(in, out)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/test/control_filters/test_rate_limiter.hpp b/test/control_filters/test_rate_limiter.hpp new file mode 100644 index 00000000..c9ba1631 --- /dev/null +++ b/test/control_filters/test_rate_limiter.hpp @@ -0,0 +1,62 @@ +// Copyright 2024 AIT - Austrian Institute of Technology GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROL_FILTERS__TEST_RATE_LIMITER_HPP_ +#define CONTROL_FILTERS__TEST_RATE_LIMITER_HPP_ + +#include +#include +#include "gmock/gmock.h" + +#include "control_filters/rate_limiter.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_rate_limiter"); +} // namespace + +class RateLimiterTest : public ::testing::Test +{ +public: + void SetUp() override + { + auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + node_ = std::make_shared(testname); + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + RateLimiterTest() + { + executor_ = std::make_shared(); + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + node_.reset(); + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + std::thread executor_thread_; +}; + +#endif // CONTROL_FILTERS__TEST_RATE_LIMITER_HPP_ diff --git a/test/control_filters/test_rate_limiter_parameters.yaml b/test/control_filters/test_rate_limiter_parameters.yaml new file mode 100644 index 00000000..6acb9d8a --- /dev/null +++ b/test/control_filters/test_rate_limiter_parameters.yaml @@ -0,0 +1,17 @@ +TestRateLimiterMissingParameter: + ros__parameters: {} + +TestRateLimiterAllParameters: + ros__parameters: + sampling_interval: 1.0 + +TestRateLimiterInvalidThenFixedParameter: + ros__parameters: {} + +TestRateLimiterThrowsUnconfigured: + ros__parameters: + sampling_interval: 1.0 + +TestRateLimiterCompute: + ros__parameters: + sampling_interval: 1.0 From e1f9b281dda3a8d7a8f087c94edc7b2b8cd70443 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 20 Nov 2024 22:07:05 +0000 Subject: [PATCH 4/8] Fix bug in memory old outputs --- include/control_filters/rate_limiter.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/control_filters/rate_limiter.hpp b/include/control_filters/rate_limiter.hpp index 16a93fd7..6e002c03 100644 --- a/include/control_filters/rate_limiter.hpp +++ b/include/control_filters/rate_limiter.hpp @@ -132,8 +132,9 @@ bool RateLimiter::update(const T & data_in, T & data_out) } T v = data_in; limiter->limit(v, v0, v1, static_cast(parameters_.sampling_interval)); + // shift the values for the next update call v1 = v0; - v0 = data_in; + v0 = v; // use the limited value data_out = v; return true; } From 8123f1f67d041c19618d8e98ba202fdac4a29a79 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 22 Nov 2024 20:29:03 +0000 Subject: [PATCH 5/8] Pass validators to CMake file --- CMakeLists.txt | 1 + include/control_filters/rate_limiter.hpp | 1 - test/control_filters/test_rate_limiter.cpp | 4 ++-- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8242a3ec..1afee7b9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,6 +81,7 @@ ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPEN generate_parameter_library( rate_limiter_parameters src/control_filters/rate_limiter_parameters.yaml + include/control_filters/custom_validators.hpp ) add_library(rate_limiter SHARED diff --git a/include/control_filters/rate_limiter.hpp b/include/control_filters/rate_limiter.hpp index 6e002c03..d72d0b23 100644 --- a/include/control_filters/rate_limiter.hpp +++ b/include/control_filters/rate_limiter.hpp @@ -23,7 +23,6 @@ #include "filters/filter_base.hpp" #include "control_toolbox/rate_limiter.hpp" -#include "custom_validators.hpp" #include "rate_limiter_parameters.hpp" namespace control_filters diff --git a/test/control_filters/test_rate_limiter.cpp b/test/control_filters/test_rate_limiter.cpp index 6ba29b60..c985efbf 100644 --- a/test/control_filters/test_rate_limiter.cpp +++ b/test/control_filters/test_rate_limiter.cpp @@ -36,7 +36,7 @@ TEST_F(RateLimiterTest, TestRateLimiterMissingParameter) std::shared_ptr> filter_ = std::make_shared>(); - // should deny configuration as sampling frequency is missing + // should deny configuration as sampling_interval is missing ASSERT_FALSE(filter_->configure("", "TestRateLimiter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); } @@ -46,7 +46,7 @@ TEST_F(RateLimiterTest, TestRateLimiterInvalidThenFixedParameter) std::shared_ptr> filter_ = std::make_shared>(); - // should deny configuration as sampling frequency is invalid + // should deny configuration as sampling_interval is invalid ASSERT_FALSE(filter_->configure("", "TestRateLimiter", node_->get_node_logging_interface(), node_->get_node_parameters_interface())); From 4b8e49faf3d8da547cba35abd656ed5112288ec6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 22 Nov 2024 20:37:04 +0000 Subject: [PATCH 6/8] Fix templated class member variable types --- include/control_toolbox/rate_limiter.hpp | 48 ++++++++++++------------ 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/include/control_toolbox/rate_limiter.hpp b/include/control_toolbox/rate_limiter.hpp index b847b5e7..abcb5aec 100644 --- a/include/control_toolbox/rate_limiter.hpp +++ b/include/control_toolbox/rate_limiter.hpp @@ -56,14 +56,14 @@ class RateLimiter * */ RateLimiter( - T min_value = std::numeric_limits::quiet_NaN(), - T max_value = std::numeric_limits::quiet_NaN(), - T min_first_derivative_neg = std::numeric_limits::quiet_NaN(), - T max_first_derivative_pos = std::numeric_limits::quiet_NaN(), - T min_first_derivative_pos = std::numeric_limits::quiet_NaN(), - T max_first_derivative_neg = std::numeric_limits::quiet_NaN(), - T min_second_derivative = std::numeric_limits::quiet_NaN(), - T max_second_derivative = std::numeric_limits::quiet_NaN()); + T min_value = std::numeric_limits::quiet_NaN(), + T max_value = std::numeric_limits::quiet_NaN(), + T min_first_derivative_neg = std::numeric_limits::quiet_NaN(), + T max_first_derivative_pos = std::numeric_limits::quiet_NaN(), + T min_first_derivative_pos = std::numeric_limits::quiet_NaN(), + T max_first_derivative_neg = std::numeric_limits::quiet_NaN(), + T min_second_derivative = std::numeric_limits::quiet_NaN(), + T max_second_derivative = std::numeric_limits::quiet_NaN()); /** * \brief Limit the value and first_derivative @@ -123,14 +123,14 @@ class RateLimiter * If min_first_derivative_pos/max_first_derivative_neg values are NAN, symmetric limits are used */ void set_params( - T min_value = std::numeric_limits::quiet_NaN(), - T max_value = std::numeric_limits::quiet_NaN(), - T min_first_derivative_neg = std::numeric_limits::quiet_NaN(), - T max_first_derivative_pos = std::numeric_limits::quiet_NaN(), - T min_first_derivative_pos = std::numeric_limits::quiet_NaN(), - T max_first_derivative_neg = std::numeric_limits::quiet_NaN(), - T min_second_derivative = std::numeric_limits::quiet_NaN(), - T max_second_derivative = std::numeric_limits::quiet_NaN()); + T min_value = std::numeric_limits::quiet_NaN(), + T max_value = std::numeric_limits::quiet_NaN(), + T min_first_derivative_neg = std::numeric_limits::quiet_NaN(), + T max_first_derivative_pos = std::numeric_limits::quiet_NaN(), + T min_first_derivative_pos = std::numeric_limits::quiet_NaN(), + T max_first_derivative_neg = std::numeric_limits::quiet_NaN(), + T min_second_derivative = std::numeric_limits::quiet_NaN(), + T max_second_derivative = std::numeric_limits::quiet_NaN()); private: // Enable/Disable value/first_derivative/second_derivative limits: @@ -139,18 +139,18 @@ class RateLimiter bool has_second_derivative_limits_ = true; // value limits: - T min_value_ = std::numeric_limits::quiet_NaN(); - T max_value_ = std::numeric_limits::quiet_NaN(); + T min_value_ = std::numeric_limits::quiet_NaN(); + T max_value_ = std::numeric_limits::quiet_NaN(); // first_derivative limits: - T min_first_derivative_neg_ = std::numeric_limits::quiet_NaN(); - T max_first_derivative_pos_ = std::numeric_limits::quiet_NaN(); - T min_first_derivative_pos_ = std::numeric_limits::quiet_NaN(); - T max_first_derivative_neg_ = std::numeric_limits::quiet_NaN(); + T min_first_derivative_neg_ = std::numeric_limits::quiet_NaN(); + T max_first_derivative_pos_ = std::numeric_limits::quiet_NaN(); + T min_first_derivative_pos_ = std::numeric_limits::quiet_NaN(); + T max_first_derivative_neg_ = std::numeric_limits::quiet_NaN(); // second_derivative limits: - T min_second_derivative_ = std::numeric_limits::quiet_NaN(); - T max_second_derivative_ = std::numeric_limits::quiet_NaN(); + T min_second_derivative_ = std::numeric_limits::quiet_NaN(); + T max_second_derivative_ = std::numeric_limits::quiet_NaN(); }; template From c6336cdd8935c56dea377e09487ef608923070a3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 22 Nov 2024 20:53:24 +0000 Subject: [PATCH 7/8] 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 #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 From 361dd0cea7b7f43e7b1406f9d661c039bf837ae6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 5 Dec 2024 21:33:14 +0000 Subject: [PATCH 8/8] Update test criterion --- test/control_filters/test_rate_limiter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/control_filters/test_rate_limiter.cpp b/test/control_filters/test_rate_limiter.cpp index d1f4e55c..bb44f666 100644 --- a/test/control_filters/test_rate_limiter.cpp +++ b/test/control_filters/test_rate_limiter.cpp @@ -81,8 +81,8 @@ TEST_F(RateLimiterTest, TestRateLimiterCompute) 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++) + // takes 14 steps to reach 0 (first (10.0/1.0) plus second derivative limits) + for (int i = 0; i < 14; i++) { ASSERT_NO_THROW(filter_->update(in, out)); EXPECT_THAT(out, ::testing::Not(::testing::DoubleNear(in, 1e-6))) << "i=" << i;