diff --git a/CMakeLists.txt b/CMakeLists.txt index 4f144e88..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 @@ -133,6 +134,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/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..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,9 +131,15 @@ 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; - v0 = data_in; + v0 = v; // use the limited value data_out = v; return true; } 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 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] + }, } 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. diff --git a/test/control_filters/test_rate_limiter.cpp b/test/control_filters/test_rate_limiter.cpp new file mode 100644 index 00000000..bb44f666 --- /dev/null +++ b/test/control_filters/test_rate_limiter.cpp @@ -0,0 +1,101 @@ +// 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_interval 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_interval 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 = 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 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; + } + ASSERT_NO_THROW(filter_->update(in, out)); + EXPECT_THAT(out, ::testing::DoubleNear(in, 1e-6)); +} + +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..b87f85d2 --- /dev/null +++ b/test/control_filters/test_rate_limiter_parameters.yaml @@ -0,0 +1,25 @@ +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 + 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