From 6a5c6dbca77a997139859eaafa29c00de41ef5a4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 8 Nov 2024 20:56:55 +0000 Subject: [PATCH 1/5] Add exponential filter plugin --- CMakeLists.txt | 28 +++- control_filters.xml | 9 ++ .../control_filters/exponential_filter.hpp | 133 ++++++++++++++++++ src/control_filters/exponential_filter.cpp | 19 +++ .../exponential_filter_parameters.yaml | 8 ++ .../test_load_exponential_filter.cpp | 44 ++++++ 6 files changed, 240 insertions(+), 1 deletion(-) create mode 100644 include/control_filters/exponential_filter.hpp create mode 100644 src/control_filters/exponential_filter.cpp create mode 100644 src/control_filters/exponential_filter_parameters.yaml create mode 100644 test/control_filters/test_load_exponential_filter.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8777918d..ca877b81 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,6 +78,25 @@ target_link_libraries(low_pass_filter PUBLIC ) ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) +generate_parameter_library( + exponential_filter_parameters + src/control_filters/exponential_filter_parameters.yaml +) + +add_library(exponential_filter SHARED + src/control_filters/exponential_filter.cpp +) +target_compile_features(exponential_filter PUBLIC cxx_std_17) +target_include_directories(exponential_filter PUBLIC + $ + $ + $ +) +target_link_libraries(exponential_filter PUBLIC + exponential_filter_parameters +) +ament_target_dependencies(exponential_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + # Install pluginlib xml pluginlib_export_plugin_description_file(filters control_filters.xml) @@ -109,6 +128,10 @@ if(BUILD_TESTING) ament_add_gmock(test_load_low_pass_filter test/control_filters/test_load_low_pass_filter.cpp) 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}) + + ament_add_gmock(test_load_exponential_filter test/control_filters/test_load_exponential_filter.cpp) + target_link_libraries(test_load_exponential_filter exponential_filter exponential_filter_parameters) + ament_target_dependencies(test_load_exponential_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) endif() # Install @@ -116,7 +139,10 @@ install( DIRECTORY include/ DESTINATION include/control_toolbox ) -install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters +install(TARGETS + control_toolbox + low_pass_filter low_pass_filter_parameters + exponential_filter exponential_filter_parameters EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/control_filters.xml b/control_filters.xml index a2280008..23d11c1b 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -15,4 +15,13 @@ + + + + This is an exponential filter working with a double value. + + + diff --git a/include/control_filters/exponential_filter.hpp b/include/control_filters/exponential_filter.hpp new file mode 100644 index 00000000..6fc2e493 --- /dev/null +++ b/include/control_filters/exponential_filter.hpp @@ -0,0 +1,133 @@ +// Copyright (c) 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__EXPONENTIAL_FILTER_HPP_ +#define CONTROL_FILTERS__EXPONENTIAL_FILTER_HPP_ + +#include +#include +#include + +#include "filters/filter_base.hpp" + +#include "control_toolbox/filters.hpp" +#include "exponential_filter_parameters.hpp" + +namespace control_filters +{ + +/***************************************************/ +/*! \class ExponentialFilter + \brief A exponential filter class. + + This class implements a low-pass filter for + various data types. + + \section Usage + + The ExponentialFilter class is meant to be instantiated as a filter in + a controller but can also be used elsewhere. + For manual instantiation, you should first call configure() + (in non-realtime) and then call update() at every update step. + +*/ +/***************************************************/ + +template +class ExponentialFilter : public filters::FilterBase +{ +public: + /*! + * \brief Configure the ExponentialFilter (access and process params). + */ + bool configure() override; + + /*! + * \brief Applies one iteration of the exponential filter. + * + * \param data_in input to the filter + * \param data_out filtered output + * + * \returns false if filter is not configured, true otherwise + */ + bool update(const T & data_in, T & data_out) override; + +private: + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr logger_; + std::shared_ptr parameter_handler_; + exponential_filter::Params parameters_; + T last_smoothed_value; +}; + +template +bool ExponentialFilter::configure() +{ + logger_.reset( + new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); + + // Initialize the parameters once + if (!parameter_handler_) + { + try + { + parameter_handler_ = + std::make_shared(this->params_interface_, + this->param_prefix_); + } + catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR((*logger_), "Exponential filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + catch (rclcpp::exceptions::InvalidParameterValueException & ex) { + RCLCPP_ERROR((*logger_), "Exponential filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + } + parameters_ = parameter_handler_->get_params(); + + last_smoothed_value = std::numeric_limits::quiet_NaN(); + + return true; +} + +template +bool ExponentialFilter::update(const T & data_in, T & data_out) +{ + if (!this->configured_) + { + throw std::runtime_error("Filter is not configured"); + } + + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + } + + if (std::isnan(last_smoothed_value)) + { + last_smoothed_value = data_in; + } + + data_out = last_smoothed_value = + filters::exponentialSmoothing(data_in, last_smoothed_value, parameters_.alpha); + return true; +} + +} // namespace control_filters + +#endif // CONTROL_FILTERS__EXPONENTIAL_FILTER_HPP_ diff --git a/src/control_filters/exponential_filter.cpp b/src/control_filters/exponential_filter.cpp new file mode 100644 index 00000000..d84bc491 --- /dev/null +++ b/src/control_filters/exponential_filter.cpp @@ -0,0 +1,19 @@ +// Copyright (c) 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 "control_filters/exponential_filter.hpp" + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(control_filters::ExponentialFilter, filters::FilterBase) diff --git a/src/control_filters/exponential_filter_parameters.yaml b/src/control_filters/exponential_filter_parameters.yaml new file mode 100644 index 00000000..19cc52fd --- /dev/null +++ b/src/control_filters/exponential_filter_parameters.yaml @@ -0,0 +1,8 @@ +exponential_filter: + alpha: { + type: double, + description: "Exponential decay factor", + validation: { + gt<>: [0.0] + }, + } diff --git a/test/control_filters/test_load_exponential_filter.cpp b/test/control_filters/test_load_exponential_filter.cpp new file mode 100644 index 00000000..8f4fe3b1 --- /dev/null +++ b/test/control_filters/test_load_exponential_filter.cpp @@ -0,0 +1,44 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 +#include +#include + +#include "rclcpp/utilities.hpp" +#include "pluginlib/class_loader.hpp" + +#include "control_filters/exponential_filter.hpp" + +TEST(TestLoadExponentialFilter, load_exponential_filter_double) +{ + rclcpp::init(0, nullptr); + + pluginlib::ClassLoader> filter_loader("filters", + "filters::FilterBase"); + std::shared_ptr> filter; + auto available_classes = filter_loader.getDeclaredClasses(); + std::stringstream sstr; + sstr << "available filters:" << std::endl; + for (const auto& available_class : available_classes) + { + sstr << " " << available_class << std::endl; + } + + std::string filter_type = "control_filters/ExponentialFilterDouble"; + ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); + EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)) << sstr.str(); + + rclcpp::shutdown(); +} From 9c7423dc9a37bfbc6adc6842fec90756620ace6d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 8 Nov 2024 20:37:40 +0000 Subject: [PATCH 2/5] Add simple tests --- CMakeLists.txt | 6 ++ .../test_exponential_filter.cpp | 58 +++++++++++++++++ .../test_exponential_filter.hpp | 62 +++++++++++++++++++ .../test_exponential_filter_parameters.yaml | 3 + 4 files changed, 129 insertions(+) create mode 100644 test/control_filters/test_exponential_filter.cpp create mode 100644 test/control_filters/test_exponential_filter.hpp create mode 100644 test/control_filters/test_exponential_filter_parameters.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index ca877b81..7b9803a4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -129,6 +129,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_exponential_filter test/control_filters/test_exponential_filter.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_exponential_filter_parameters.yaml + ) + target_link_libraries(test_exponential_filter exponential_filter exponential_filter_parameters) + ament_target_dependencies(test_exponential_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + ament_add_gmock(test_load_exponential_filter test/control_filters/test_load_exponential_filter.cpp) target_link_libraries(test_load_exponential_filter exponential_filter exponential_filter_parameters) ament_target_dependencies(test_load_exponential_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) diff --git a/test/control_filters/test_exponential_filter.cpp b/test/control_filters/test_exponential_filter.cpp new file mode 100644 index 00000000..b31115f3 --- /dev/null +++ b/test/control_filters/test_exponential_filter.cpp @@ -0,0 +1,58 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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_exponential_filter.hpp" + +TEST_F(ExponentialFilterTest, TestExponentialFilterThrowsUnconfigured) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + double in, out; + ASSERT_THROW(filter_->update(in, out), std::runtime_error); +} + + +TEST_F(ExponentialFilterTest, TestExponentialFilterComputation) +{ + // parameters should match the test yaml file + double alpha = 1000.0; + + double in = 1.0, calculated, out; + + std::shared_ptr> filter_ = + std::make_shared>(); + + // configure + ASSERT_TRUE(filter_->configure("", "TestExponentialFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // first filter pass, output should be input value as no old value was stored + ASSERT_TRUE(filter_->update(in, out)); + ASSERT_EQ(out, 1.0); + + // second filter pass with same values: no change + // check equality with low-pass-filter + ASSERT_TRUE(filter_->update(in, out)); + calculated = in; + ASSERT_EQ(calculated, 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_exponential_filter.hpp b/test/control_filters/test_exponential_filter.hpp new file mode 100644 index 00000000..edee5a7a --- /dev/null +++ b/test/control_filters/test_exponential_filter.hpp @@ -0,0 +1,62 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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_EXPONENTIAL_FILTER_HPP_ +#define CONTROL_FILTERS__TEST_EXPONENTIAL_FILTER_HPP_ + +#include +#include +#include "gmock/gmock.h" + +#include "control_filters/exponential_filter.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_exponential_filter"); +} // namespace + +class ExponentialFilterTest : 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(); }); + } + + ExponentialFilterTest() + { + 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_EXPONENTIAL_FILTER_HPP_ diff --git a/test/control_filters/test_exponential_filter_parameters.yaml b/test/control_filters/test_exponential_filter_parameters.yaml new file mode 100644 index 00000000..ae769555 --- /dev/null +++ b/test/control_filters/test_exponential_filter_parameters.yaml @@ -0,0 +1,3 @@ +TestExponentialFilterComputation: + ros__parameters: + alpha: 1000.0 From 4767f68cfb05a5a64c0cb9169483042c08885840 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 8 Nov 2024 20:37:46 +0000 Subject: [PATCH 3/5] Update readme --- src/control_filters/README.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/control_filters/README.md b/src/control_filters/README.md index cac37a26..6e535746 100644 --- a/src/control_filters/README.md +++ b/src/control_filters/README.md @@ -1,8 +1,11 @@ # Control filters +Implement filter plugins for control purposes as https://index.ros.org/r/filters/github-ros-filters/ + ## Available filters * Low Pass: implements a low-pass filter based on a time-invariant [Infinite Impulse Response (IIR) filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), for different data types (doubles or wrench). +* Exponential Filter: Exponential filter for double data type. ## Low Pass filter @@ -29,3 +32,13 @@ with * a the feedbackward coefficient such that a = exp( -1/sf (2 pi df) / (10^(di/-10)) ) * b the feedforward coefficient such that b = 1 - a + + +## Exponential filter + +### Required parameters +* `alpha`: the exponential decay factor + +### Algorithm + + smoothed_value = alpha * current_value + (1 - alpha) * last_smoothed_value; From f2e9cd819ab0e1d2dff7efe5eb6f081513aec670 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 8 Nov 2024 20:43:34 +0000 Subject: [PATCH 4/5] Improve parameter check and test --- src/control_filters/exponential_filter_parameters.yaml | 2 +- test/control_filters/test_exponential_filter.cpp | 9 ++++++++- .../test_exponential_filter_parameters.yaml | 2 +- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/control_filters/exponential_filter_parameters.yaml b/src/control_filters/exponential_filter_parameters.yaml index 19cc52fd..227ddf26 100644 --- a/src/control_filters/exponential_filter_parameters.yaml +++ b/src/control_filters/exponential_filter_parameters.yaml @@ -3,6 +3,6 @@ exponential_filter: type: double, description: "Exponential decay factor", validation: { - gt<>: [0.0] + bounds<>: [0.0, 1.0] }, } diff --git a/test/control_filters/test_exponential_filter.cpp b/test/control_filters/test_exponential_filter.cpp index b31115f3..73f65a7d 100644 --- a/test/control_filters/test_exponential_filter.cpp +++ b/test/control_filters/test_exponential_filter.cpp @@ -26,7 +26,7 @@ TEST_F(ExponentialFilterTest, TestExponentialFilterThrowsUnconfigured) TEST_F(ExponentialFilterTest, TestExponentialFilterComputation) { // parameters should match the test yaml file - double alpha = 1000.0; + double alpha = 0.7; double in = 1.0, calculated, out; @@ -46,6 +46,13 @@ TEST_F(ExponentialFilterTest, TestExponentialFilterComputation) ASSERT_TRUE(filter_->update(in, out)); calculated = in; ASSERT_EQ(calculated, out); + + // input change + in = 0.0; + for (int i = 0; i < 100; ++i){ + ASSERT_TRUE(filter_->update(in, out)); + calculated = alpha * in + (1 - alpha) * calculated; + ASSERT_EQ(calculated, out);} } int main(int argc, char ** argv) diff --git a/test/control_filters/test_exponential_filter_parameters.yaml b/test/control_filters/test_exponential_filter_parameters.yaml index ae769555..4bed6865 100644 --- a/test/control_filters/test_exponential_filter_parameters.yaml +++ b/test/control_filters/test_exponential_filter_parameters.yaml @@ -1,3 +1,3 @@ TestExponentialFilterComputation: ros__parameters: - alpha: 1000.0 + alpha: 0.7 From d877ba675c8b74b06fcc336704ee3152b5f178aa Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 8 Nov 2024 20:45:45 +0000 Subject: [PATCH 5/5] Update copyright --- test/control_filters/test_exponential_filter.cpp | 2 +- test/control_filters/test_exponential_filter.hpp | 2 +- test/control_filters/test_load_exponential_filter.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/test/control_filters/test_exponential_filter.cpp b/test/control_filters/test_exponential_filter.cpp index 73f65a7d..7c5c9b5f 100644 --- a/test/control_filters/test_exponential_filter.cpp +++ b/test/control_filters/test_exponential_filter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 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_exponential_filter.hpp b/test/control_filters/test_exponential_filter.hpp index edee5a7a..ab35fdfb 100644 --- a/test/control_filters/test_exponential_filter.hpp +++ b/test/control_filters/test_exponential_filter.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 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_load_exponential_filter.cpp b/test/control_filters/test_load_exponential_filter.cpp index 8f4fe3b1..1b82ac44 100644 --- a/test/control_filters/test_load_exponential_filter.cpp +++ b/test/control_filters/test_load_exponential_filter.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 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.