From e3fde82033ef63926b1474521d58a5148fb6c7c8 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Sat, 2 Mar 2024 21:22:11 +0100 Subject: [PATCH 1/7] Make ros-independent version of LPF --- control_filters.xml | 4 +- include/control_filters/low_pass_filter.hpp | 70 ++----- .../control_filters/low_pass_filter_ros.hpp | 192 ++++++++++++++++++ src/control_filters/low_pass_filter.cpp | 6 +- test/control_filters/test_low_pass_filter.cpp | 8 +- test/control_filters/test_low_pass_filter.hpp | 2 +- 6 files changed, 222 insertions(+), 60 deletions(-) create mode 100644 include/control_filters/low_pass_filter_ros.hpp diff --git a/control_filters.xml b/control_filters.xml index a2280008..0d9e292d 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,14 +1,14 @@ This is a low pass filter working with a double value. This is a low pass filter working with geometry_msgs::WrenchStamped. diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index e4a11782..fb64035a 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -21,7 +21,6 @@ #include #include -#include "low_pass_filter_parameters.hpp" #include "filters/filter_base.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" @@ -82,6 +81,10 @@ class LowPassFilter : public filters::FilterBase // Default constructor LowPassFilter(); + LowPassFilter(double sampling_frequency, double damping_frequency, double damping_intensity){ + set_params(sampling_frequency, damping_frequency, damping_intensity); + } + /*! * \brief Destructor of LowPassFilter class. */ @@ -102,6 +105,19 @@ class LowPassFilter : public filters::FilterBase */ bool update(const T & data_in, T & data_out) override; + bool set_params( + const double sampling_frequency, + const double damping_frequency, + const double damping_intensity) + { + // TODO(roncapat): parameters validation + this->sampling_frequency = sampling_frequency; + this->damping_frequency = damping_frequency; + this->damping_intensity = damping_intensity; + compute_internal_params(); + return true; + } + protected: /*! * \brief Internal computation of the feedforward and feedbackward coefficients @@ -110,22 +126,18 @@ class LowPassFilter : public filters::FilterBase void compute_internal_params() { a1_ = exp( - -1.0 / parameters_.sampling_frequency * (2.0 * M_PI * parameters_.damping_frequency) / - (pow(10.0, parameters_.damping_intensity / -10.0))); + -1.0 / sampling_frequency * (2.0 * M_PI * damping_frequency) / + (pow(10.0, damping_intensity / -10.0))); b1_ = 1.0 - a1_; }; private: - rclcpp::Clock::SharedPtr clock_; - std::shared_ptr logger_; - std::shared_ptr parameter_handler_; - low_pass_filter::Params parameters_; - // Filter parameters /** internal data storage (double). */ double filtered_value, filtered_old_value, old_value; /** internal data storage (wrench). */ Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; + double sampling_frequency, damping_frequency, damping_intensity; double a1_; /**< feedbackward coefficient. */ double b1_; /**< feedforward coefficient. */ }; @@ -143,31 +155,6 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() { - clock_ = std::make_shared(RCL_SYSTEM_TIME); - 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_), "LowPass filter cannot be configured: %s", ex.what()); - parameter_handler_.reset(); - return false; - } - catch (rclcpp::exceptions::InvalidParameterValueException & ex) { - RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what()); - parameter_handler_.reset(); - return false; - } - } - parameters_ = parameter_handler_->get_params(); compute_internal_params(); // Initialize storage Vectors @@ -187,18 +174,9 @@ inline bool LowPassFilter::update( { if (!this->configured_) { - if (logger_) - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); return false; } - // Update internal parameters if required - if (parameter_handler_->is_old(parameters_)) - { - parameters_ = parameter_handler_->get_params(); - compute_internal_params(); - } - // IIR Filter msg_filtered = b1_ * msg_old + a1_ * msg_filtered_old; msg_filtered_old = msg_filtered; @@ -228,17 +206,9 @@ bool LowPassFilter::update(const T & data_in, T & data_out) { if (!this->configured_) { - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); return false; } - // Update internal parameters if required - if (parameter_handler_->is_old(parameters_)) - { - parameters_ = parameter_handler_->get_params(); - compute_internal_params(); - } - // Filter data_out = b1_ * old_value + a1_ * filtered_old_value; filtered_old_value = data_out; diff --git a/include/control_filters/low_pass_filter_ros.hpp b/include/control_filters/low_pass_filter_ros.hpp new file mode 100644 index 00000000..62dc22fd --- /dev/null +++ b/include/control_filters/low_pass_filter_ros.hpp @@ -0,0 +1,192 @@ +// 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__LOW_PASS_FILTER_ROS_HPP_ +#define CONTROL_FILTERS__LOW_PASS_FILTER_ROS_HPP_ + +#include +#include +#include +#include +#include + +#include "low_pass_filter.hpp" +#include "low_pass_filter_parameters.hpp" + +#include "geometry_msgs/msg/wrench_stamped.hpp" + +namespace control_filters +{ + +/***************************************************/ +/*! \class LowPassFilterRos + \brief A Low-pass filter class. + + This class implements a low-pass filter for + various data types based on an Infinite Impulse Response Filter. + For vector elements, the filtering is applied separately on + each element of the vector. + + In particular, this class implements a simplified version of + an IIR filter equation : + + \f$y(n) = b x(n-1) + a y(n-1)\f$ + + where:
+
    +
  • \f$ x(n)\f$ is the input signal +
  • \f$ y(n)\f$ is the output signal (filtered) +
  • \f$ b \f$ is the feedforward filter coefficient +
  • \f$ a \f$ is the feedback filter coefficient +
+ + and the Low-Pass coefficient equation: +
+
    +
  • \f$ a = e^{\frac{-1}{sf} \frac{2\pi df}{10^{\frac{di}{-10}}}} \f$ +
  • \f$ b = 1 - a \f$ +
+ + where:
+
    +
  • \f$ sf \f$ is the sampling frequency +
  • \f$ df \f$ is the damping frequency +
  • \f$ di \f$ is the damping intensity (amplitude) +
+ + \section Usage + + The LowPassFilterRos 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 LowPassFilterRos : public LowPassFilter +{ +public: + /*! + * \brief Configure the LowPassFilterRos (access and process params). + */ + bool configure() override; + + /*! + * \brief Applies one iteration of the IIR 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_; + low_pass_filter::Params parameters_; +}; + +template +bool LowPassFilterRos::configure() +{ + clock_ = std::make_shared(RCL_SYSTEM_TIME); + 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_), "LowPass filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + catch (rclcpp::exceptions::InvalidParameterValueException & ex) { + RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + } + parameters_ = parameter_handler_->get_params(); + LowPassFilter::set_params( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); + LowPassFilter::compute_internal_params(); + + return LowPassFilter::configure(); +} + +template <> +inline bool LowPassFilterRos::update( + const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) +{ + if (!this->configured_) + { + if (logger_) + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); + return false; + } + + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + LowPassFilter::set_params( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); + LowPassFilter::compute_internal_params(); + } + + return LowPassFilter::update(data_in, data_out); +} + +template +bool LowPassFilterRos::update(const T & data_in, T & data_out) +{ + if (!this->configured_) + { + if (logger_) + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); + return false; + } + + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + LowPassFilter::set_params( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); + LowPassFilter::compute_internal_params(); + } + + return LowPassFilter::update(data_in, data_out); +} + +} // namespace control_filters + +#endif // CONTROL_FILTERS__LOW_PASS_FILTER_ROS_HPP_ diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter.cpp index d5063722..2fcac8d4 100644 --- a/src/control_filters/low_pass_filter.cpp +++ b/src/control_filters/low_pass_filter.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "control_filters/low_pass_filter.hpp" +#include "control_filters/low_pass_filter_ros.hpp" #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilterRos, filters::FilterBase) PLUGINLIB_EXPORT_CLASS( - control_filters::LowPassFilter, + control_filters::LowPassFilterRos, filters::FilterBase) diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp index 4f2cc9ce..658e0c2e 100644 --- a/test/control_filters/test_low_pass_filter.cpp +++ b/test/control_filters/test_low_pass_filter.cpp @@ -17,7 +17,7 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterAllParameters) { std::shared_ptr> filter_ = - std::make_shared>(); + std::make_shared>(); // should allow configuration and find parameters in sensor_filter_chain param namespace ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", @@ -34,7 +34,7 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterAllParameters) TEST_F(LowPassFilterTest, TestLowPassWrenchFilterMissingParameter) { std::shared_ptr> filter_ = - std::make_shared>(); + std::make_shared>(); // should deny configuration as sampling frequency is missing ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", @@ -44,7 +44,7 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterMissingParameter) TEST_F(LowPassFilterTest, TestLowPassWrenchFilterInvalidThenFixedParameter) { std::shared_ptr> filter_ = - std::make_shared>(); + std::make_shared>(); // should deny configuration as sampling frequency is invalid ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", @@ -75,7 +75,7 @@ TEST_F(LowPassFilterTest, TestLowPassFilterComputation) in.wrench.torque.x = 10.0; std::shared_ptr> filter_ = - std::make_shared>(); + std::make_shared>(); // not yet configured, should deny update ASSERT_FALSE(filter_->update(in, out)); diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp index c81e495c..29dc6249 100644 --- a/test/control_filters/test_low_pass_filter.hpp +++ b/test/control_filters/test_low_pass_filter.hpp @@ -19,7 +19,7 @@ #include #include "gmock/gmock.h" -#include "control_filters/low_pass_filter.hpp" +#include "control_filters/low_pass_filter_ros.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp/rclcpp.hpp" From 7cd9576103f69e7d79b432a63a1a31dd79c9eb49 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 4 Nov 2024 20:43:11 +0000 Subject: [PATCH 2/7] Make the standalone version independent of ros --- CMakeLists.txt | 10 +++---- include/control_filters/low_pass_filter.hpp | 22 ++++++++------- .../control_filters/low_pass_filter_ros.hpp | 27 +++++++++---------- ...ass_filter.cpp => low_pass_filter_ros.cpp} | 0 ....cpp => test_load_low_pass_filter_ros.cpp} | 5 ++-- ...ilter.cpp => test_low_pass_filter_ros.cpp} | 2 +- ...ilter.hpp => test_low_pass_filter_ros.hpp} | 6 ++--- 7 files changed, 38 insertions(+), 34 deletions(-) rename src/control_filters/{low_pass_filter.cpp => low_pass_filter_ros.cpp} (100%) rename test/control_filters/{test_load_low_pass_filter.cpp => test_load_low_pass_filter_ros.cpp} (96%) rename test/control_filters/{test_low_pass_filter.cpp => test_low_pass_filter_ros.cpp} (99%) rename test/control_filters/{test_low_pass_filter.hpp => test_low_pass_filter_ros.hpp} (90%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9786821c..533389ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,7 +63,7 @@ generate_parameter_library( ) add_library(low_pass_filter SHARED - src/control_filters/low_pass_filter.cpp + src/control_filters/low_pass_filter_ros.cpp ) target_compile_features(low_pass_filter PUBLIC cxx_std_17) target_include_directories(low_pass_filter PUBLIC @@ -99,13 +99,13 @@ if(BUILD_TESTING) ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) ## Control Filters - add_rostest_with_parameters_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp + add_rostest_with_parameters_gmock(test_low_pass_filter_ros test/control_filters/test_low_pass_filter_ros.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_low_pass_filter_parameters.yaml ) - target_link_libraries(test_low_pass_filter low_pass_filter low_pass_filter_parameters) - ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + target_link_libraries(test_low_pass_filter_ros low_pass_filter low_pass_filter_parameters) + ament_target_dependencies(test_low_pass_filter_ros ${CONTROL_FILTERS_INCLUDE_DEPENDS}) - ament_add_gmock(test_load_low_pass_filter test/control_filters/test_load_low_pass_filter.cpp) + ament_add_gmock(test_load_low_pass_filter test/control_filters/test_load_low_pass_filter_ros.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}) endif() diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index fb64035a..df8a97af 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -21,8 +21,6 @@ #include #include -#include "filters/filter_base.hpp" - #include "geometry_msgs/msg/wrench_stamped.hpp" namespace control_filters @@ -75,7 +73,7 @@ namespace control_filters /***************************************************/ template -class LowPassFilter : public filters::FilterBase +class LowPassFilter { public: // Default constructor @@ -88,12 +86,12 @@ class LowPassFilter : public filters::FilterBase /*! * \brief Destructor of LowPassFilter class. */ - ~LowPassFilter() override; + ~LowPassFilter(); /*! * \brief Configure the LowPassFilter (access and process params). */ - bool configure() override; + bool configure(); /*! * \brief Applies one iteration of the IIR filter. @@ -103,7 +101,7 @@ class LowPassFilter : public filters::FilterBase * * \returns false if filter is not configured, true otherwise */ - bool update(const T & data_in, T & data_out) override; + bool update(const T & data_in, T & data_out); bool set_params( const double sampling_frequency, @@ -118,6 +116,11 @@ class LowPassFilter : public filters::FilterBase return true; } + bool is_configured() const + { + return configured_; + } + protected: /*! * \brief Internal computation of the feedforward and feedbackward coefficients @@ -140,6 +143,7 @@ class LowPassFilter : public filters::FilterBase double sampling_frequency, damping_frequency, damping_intensity; double a1_; /**< feedbackward coefficient. */ double b1_; /**< feedforward coefficient. */ + bool configured_ = false; }; template @@ -165,14 +169,14 @@ bool LowPassFilter::configure() msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0; } - return true; + return configured_ = true; } template <> inline bool LowPassFilter::update( const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { - if (!this->configured_) + if (!configured_) { return false; } @@ -204,7 +208,7 @@ inline bool LowPassFilter::update( template bool LowPassFilter::update(const T & data_in, T & data_out) { - if (!this->configured_) + if (!configured_) { return false; } diff --git a/include/control_filters/low_pass_filter_ros.hpp b/include/control_filters/low_pass_filter_ros.hpp index 62dc22fd..a9b040ae 100644 --- a/include/control_filters/low_pass_filter_ros.hpp +++ b/include/control_filters/low_pass_filter_ros.hpp @@ -21,11 +21,12 @@ #include #include +#include "filters/filter_base.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" + #include "low_pass_filter.hpp" #include "low_pass_filter_parameters.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" - namespace control_filters { @@ -76,7 +77,7 @@ namespace control_filters /***************************************************/ template -class LowPassFilterRos : public LowPassFilter +class LowPassFilterRos : public filters::FilterBase { public: /*! @@ -99,6 +100,7 @@ class LowPassFilterRos : public LowPassFilter std::shared_ptr logger_; std::shared_ptr parameter_handler_; low_pass_filter::Params parameters_; + std::shared_ptr> lpf_; }; template @@ -129,20 +131,19 @@ bool LowPassFilterRos::configure() } } parameters_ = parameter_handler_->get_params(); - LowPassFilter::set_params( + lpf_ = std::make_shared>( parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity); - LowPassFilter::compute_internal_params(); - return LowPassFilter::configure(); + return lpf_->configure(); } template <> inline bool LowPassFilterRos::update( const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { - if (!this->configured_) + if (!this->configured_ || !lpf_ || !lpf_->is_configured()) { if (logger_) RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); @@ -153,20 +154,19 @@ inline bool LowPassFilterRos::update( if (parameter_handler_->is_old(parameters_)) { parameters_ = parameter_handler_->get_params(); - LowPassFilter::set_params( + lpf_->set_params( parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity); - LowPassFilter::compute_internal_params(); } - return LowPassFilter::update(data_in, data_out); + return lpf_->update(data_in, data_out); } template bool LowPassFilterRos::update(const T & data_in, T & data_out) { - if (!this->configured_) + if (!this->configured_ || !lpf_ || !lpf_->is_configured()) { if (logger_) RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); @@ -177,14 +177,13 @@ bool LowPassFilterRos::update(const T & data_in, T & data_out) if (parameter_handler_->is_old(parameters_)) { parameters_ = parameter_handler_->get_params(); - LowPassFilter::set_params( + lpf_->set_params( parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity); - LowPassFilter::compute_internal_params(); } - return LowPassFilter::update(data_in, data_out); + return lpf_->update(data_in, data_out); } } // namespace control_filters diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter_ros.cpp similarity index 100% rename from src/control_filters/low_pass_filter.cpp rename to src/control_filters/low_pass_filter_ros.cpp diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter_ros.cpp similarity index 96% rename from test/control_filters/test_load_low_pass_filter.cpp rename to test/control_filters/test_load_low_pass_filter_ros.cpp index f3c00fa8..f49b56f8 100644 --- a/test/control_filters/test_load_low_pass_filter.cpp +++ b/test/control_filters/test_load_low_pass_filter_ros.cpp @@ -15,11 +15,12 @@ #include #include #include -#include "control_filters/low_pass_filter.hpp" + #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp/utilities.hpp" -#include +#include "pluginlib/class_loader.hpp" +#include "control_filters/low_pass_filter_ros.hpp" TEST(TestLoadLowPassFilter, load_low_pass_filter_double) { diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter_ros.cpp similarity index 99% rename from test/control_filters/test_low_pass_filter.cpp rename to test/control_filters/test_low_pass_filter_ros.cpp index 658e0c2e..0efaf5c6 100644 --- a/test/control_filters/test_low_pass_filter.cpp +++ b/test/control_filters/test_low_pass_filter_ros.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_low_pass_filter.hpp" +#include "test_low_pass_filter_ros.hpp" TEST_F(LowPassFilterTest, TestLowPassWrenchFilterAllParameters) { diff --git a/test/control_filters/test_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter_ros.hpp similarity index 90% rename from test/control_filters/test_low_pass_filter.hpp rename to test/control_filters/test_low_pass_filter_ros.hpp index 29dc6249..f1dcae0a 100644 --- a/test/control_filters/test_low_pass_filter.hpp +++ b/test/control_filters/test_low_pass_filter_ros.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ -#define CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ +#ifndef CONTROL_FILTERS__TEST_LOW_PASS_FILTER_ROS_HPP_ +#define CONTROL_FILTERS__TEST_LOW_PASS_FILTER_ROS_HPP_ #include #include @@ -60,4 +60,4 @@ class LowPassFilterTest : public ::testing::Test std::thread executor_thread_; }; -#endif // CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ +#endif // CONTROL_FILTERS__TEST_LOW_PASS_FILTER_ROS_HPP_ From 1ec808b330802ba1ee9508155d8121eb29bfbf0d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 4 Nov 2024 21:13:01 +0000 Subject: [PATCH 3/7] Rename to *Base --- ...ss_filter.hpp => low_pass_filter_base.hpp} | 34 +++++++++---------- .../control_filters/low_pass_filter_ros.hpp | 6 ++-- .../test_load_low_pass_filter_ros.cpp | 4 +-- 3 files changed, 21 insertions(+), 23 deletions(-) rename include/control_filters/{low_pass_filter.hpp => low_pass_filter_base.hpp} (85%) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter_base.hpp similarity index 85% rename from include/control_filters/low_pass_filter.hpp rename to include/control_filters/low_pass_filter_base.hpp index df8a97af..977b6a70 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter_base.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ -#define CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ +#ifndef CONTROL_FILTERS__LOW_PASS_FILTER_BASE_HPP_ +#define CONTROL_FILTERS__LOW_PASS_FILTER_BASE_HPP_ #include #include @@ -27,7 +27,7 @@ namespace control_filters { /***************************************************/ -/*! \class LowPassFilter +/*! \class LowPassFilterBase \brief A Low-pass filter class. This class implements a low-pass filter for @@ -64,8 +64,6 @@ namespace control_filters \section Usage - The LowPassFilter 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. @@ -73,23 +71,23 @@ namespace control_filters /***************************************************/ template -class LowPassFilter +class LowPassFilterBase { public: // Default constructor - LowPassFilter(); + LowPassFilterBase(); - LowPassFilter(double sampling_frequency, double damping_frequency, double damping_intensity){ + LowPassFilterBase(double sampling_frequency, double damping_frequency, double damping_intensity){ set_params(sampling_frequency, damping_frequency, damping_intensity); } /*! - * \brief Destructor of LowPassFilter class. + * \brief Destructor of LowPassFilterBase class. */ - ~LowPassFilter(); + ~LowPassFilterBase(); /*! - * \brief Configure the LowPassFilter (access and process params). + * \brief Configure the LowPassFilterBase (access and process params). */ bool configure(); @@ -124,7 +122,7 @@ class LowPassFilter protected: /*! * \brief Internal computation of the feedforward and feedbackward coefficients - * according to the LowPassFilter parameters. + * according to the LowPassFilterBase parameters. */ void compute_internal_params() { @@ -147,17 +145,17 @@ class LowPassFilter }; template -LowPassFilter::LowPassFilter() : a1_(1.0), b1_(0.0) +LowPassFilterBase::LowPassFilterBase() : a1_(1.0), b1_(0.0) { } template -LowPassFilter::~LowPassFilter() +LowPassFilterBase::~LowPassFilterBase() { } template -bool LowPassFilter::configure() +bool LowPassFilterBase::configure() { compute_internal_params(); @@ -173,7 +171,7 @@ bool LowPassFilter::configure() } template <> -inline bool LowPassFilter::update( +inline bool LowPassFilterBase::update( const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { if (!configured_) @@ -206,7 +204,7 @@ inline bool LowPassFilter::update( } template -bool LowPassFilter::update(const T & data_in, T & data_out) +bool LowPassFilterBase::update(const T & data_in, T & data_out) { if (!configured_) { @@ -223,4 +221,4 @@ bool LowPassFilter::update(const T & data_in, T & data_out) } // namespace control_filters -#endif // CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ +#endif // CONTROL_FILTERS__LOW_PASS_FILTER_BASE_HPP_ diff --git a/include/control_filters/low_pass_filter_ros.hpp b/include/control_filters/low_pass_filter_ros.hpp index a9b040ae..2b9be803 100644 --- a/include/control_filters/low_pass_filter_ros.hpp +++ b/include/control_filters/low_pass_filter_ros.hpp @@ -24,7 +24,7 @@ #include "filters/filter_base.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "low_pass_filter.hpp" +#include "low_pass_filter_base.hpp" #include "low_pass_filter_parameters.hpp" namespace control_filters @@ -100,7 +100,7 @@ class LowPassFilterRos : public filters::FilterBase std::shared_ptr logger_; std::shared_ptr parameter_handler_; low_pass_filter::Params parameters_; - std::shared_ptr> lpf_; + std::shared_ptr> lpf_; }; template @@ -131,7 +131,7 @@ bool LowPassFilterRos::configure() } } parameters_ = parameter_handler_->get_params(); - lpf_ = std::make_shared>( + lpf_ = std::make_shared>( parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity); diff --git a/test/control_filters/test_load_low_pass_filter_ros.cpp b/test/control_filters/test_load_low_pass_filter_ros.cpp index f49b56f8..fab3667a 100644 --- a/test/control_filters/test_load_low_pass_filter_ros.cpp +++ b/test/control_filters/test_load_low_pass_filter_ros.cpp @@ -39,7 +39,7 @@ TEST(TestLoadLowPassFilter, load_low_pass_filter_double) std::string filter_type = "control_filters/LowPassFilterDouble"; ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); - ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); rclcpp::shutdown(); } @@ -61,7 +61,7 @@ TEST(TestLoadLowPassFilter, load_low_pass_filter_wrench) std::string filter_type = "control_filters/LowPassFilterWrench"; ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); - ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + EXPECT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); rclcpp::shutdown(); } From e04739b31801056c68147556b543843659729658 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 4 Nov 2024 21:14:31 +0000 Subject: [PATCH 4/7] Backward compatibility --- CMakeLists.txt | 16 ++++----- control_filters.xml | 2 +- include/control_filters/low_pass_filter.hpp | 37 +++++++++++++++++++++ 3 files changed, 46 insertions(+), 9 deletions(-) create mode 100644 include/control_filters/low_pass_filter.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 533389ab..208cbd1b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,20 +62,20 @@ generate_parameter_library( src/control_filters/low_pass_filter_parameters.yaml ) -add_library(low_pass_filter SHARED +add_library(low_pass_filter_ros SHARED src/control_filters/low_pass_filter_ros.cpp ) -target_compile_features(low_pass_filter PUBLIC cxx_std_17) -target_include_directories(low_pass_filter PUBLIC +target_compile_features(low_pass_filter_ros PUBLIC cxx_std_17) +target_include_directories(low_pass_filter_ros PUBLIC $ $ $ ) -target_link_libraries(low_pass_filter PUBLIC +target_link_libraries(low_pass_filter_ros PUBLIC low_pass_filter_parameters Eigen3::Eigen ) -ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) +ament_target_dependencies(low_pass_filter_ros PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) # Install pluginlib xml pluginlib_export_plugin_description_file(filters control_filters.xml) @@ -102,11 +102,11 @@ if(BUILD_TESTING) add_rostest_with_parameters_gmock(test_low_pass_filter_ros test/control_filters/test_low_pass_filter_ros.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_low_pass_filter_parameters.yaml ) - target_link_libraries(test_low_pass_filter_ros low_pass_filter low_pass_filter_parameters) + target_link_libraries(test_low_pass_filter_ros low_pass_filter_ros low_pass_filter_parameters) ament_target_dependencies(test_low_pass_filter_ros ${CONTROL_FILTERS_INCLUDE_DEPENDS}) ament_add_gmock(test_load_low_pass_filter test/control_filters/test_load_low_pass_filter_ros.cpp) - target_link_libraries(test_load_low_pass_filter low_pass_filter low_pass_filter_parameters) + target_link_libraries(test_load_low_pass_filter low_pass_filter_ros low_pass_filter_parameters) ament_target_dependencies(test_load_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) endif() @@ -115,7 +115,7 @@ 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_ros low_pass_filter_parameters EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/control_filters.xml b/control_filters.xml index 0d9e292d..dca303f8 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,5 +1,5 @@ - + diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp new file mode 100644 index 00000000..408e13b2 --- /dev/null +++ b/include/control_filters/low_pass_filter.hpp @@ -0,0 +1,37 @@ +// 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__LOW_PASS_FILTER_HPP_ +#define CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ + +#include "low_pass_filter_ros.hpp" + +// TODO(christophfroehlich) file is only for backwards compatibility, remove this file in the future + +// Deprecation notice +#ifdef _WIN32 +#pragma message( \ + "This header include is deprecated. Please update your code to use 'low_pass_filter_ros.hpp' header and link against 'control_toolbox' library.") //NOLINT +#else +#warning \ + "This header include is deprecated. Please update your code to use 'low_pass_filter_ros.hpp' header and link against 'control_toolbox' library." //NOLINT +#endif + +namespace control_filters +{ +template +using LowPassFilter = control_filters::LowPassFilterRos; +} // namespace control_filters + +#endif // CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ From 3171fb0ca6cb8cd9ae42e651d4eda633f6348713 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 4 Nov 2024 22:13:19 +0000 Subject: [PATCH 5/7] Rename base class again and move to different namespace --- include/control_filters/low_pass_filter.hpp | 4 +- .../control_filters/low_pass_filter_ros.hpp | 6 +-- .../low_pass_filter.hpp} | 40 +++++++++---------- 3 files changed, 25 insertions(+), 25 deletions(-) rename include/{control_filters/low_pass_filter_base.hpp => control_toolbox/low_pass_filter.hpp} (83%) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index 408e13b2..68d12611 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -22,10 +22,10 @@ // Deprecation notice #ifdef _WIN32 #pragma message( \ - "This header include is deprecated. Please update your code to use 'low_pass_filter_ros.hpp' header and link against 'control_toolbox' library.") //NOLINT + "This header include is deprecated. Please update your code to use 'control_toolbox/low_pass_filter_ros.hpp' header.") //NOLINT #else #warning \ - "This header include is deprecated. Please update your code to use 'low_pass_filter_ros.hpp' header and link against 'control_toolbox' library." //NOLINT + "This header include is deprecated. Please update your code to use 'control_toolbox/low_pass_filter_ros.hpp' header." //NOLINT #endif namespace control_filters diff --git a/include/control_filters/low_pass_filter_ros.hpp b/include/control_filters/low_pass_filter_ros.hpp index 2b9be803..ceab6c2c 100644 --- a/include/control_filters/low_pass_filter_ros.hpp +++ b/include/control_filters/low_pass_filter_ros.hpp @@ -24,7 +24,7 @@ #include "filters/filter_base.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" -#include "low_pass_filter_base.hpp" +#include "control_toolbox/low_pass_filter.hpp" #include "low_pass_filter_parameters.hpp" namespace control_filters @@ -100,7 +100,7 @@ class LowPassFilterRos : public filters::FilterBase std::shared_ptr logger_; std::shared_ptr parameter_handler_; low_pass_filter::Params parameters_; - std::shared_ptr> lpf_; + std::shared_ptr> lpf_; }; template @@ -131,7 +131,7 @@ bool LowPassFilterRos::configure() } } parameters_ = parameter_handler_->get_params(); - lpf_ = std::make_shared>( + lpf_ = std::make_shared>( parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity); diff --git a/include/control_filters/low_pass_filter_base.hpp b/include/control_toolbox/low_pass_filter.hpp similarity index 83% rename from include/control_filters/low_pass_filter_base.hpp rename to include/control_toolbox/low_pass_filter.hpp index 977b6a70..df214d09 100644 --- a/include/control_filters/low_pass_filter_base.hpp +++ b/include/control_toolbox/low_pass_filter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_FILTERS__LOW_PASS_FILTER_BASE_HPP_ -#define CONTROL_FILTERS__LOW_PASS_FILTER_BASE_HPP_ +#ifndef CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ +#define CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ #include #include @@ -23,11 +23,11 @@ #include "geometry_msgs/msg/wrench_stamped.hpp" -namespace control_filters +namespace control_toolbox { /***************************************************/ -/*! \class LowPassFilterBase +/*! \class LowPassFilter \brief A Low-pass filter class. This class implements a low-pass filter for @@ -71,23 +71,23 @@ namespace control_filters /***************************************************/ template -class LowPassFilterBase +class LowPassFilter { public: // Default constructor - LowPassFilterBase(); + LowPassFilter(); - LowPassFilterBase(double sampling_frequency, double damping_frequency, double damping_intensity){ + LowPassFilter(double sampling_frequency, double damping_frequency, double damping_intensity){ set_params(sampling_frequency, damping_frequency, damping_intensity); } /*! - * \brief Destructor of LowPassFilterBase class. + * \brief Destructor of LowPassFilter class. */ - ~LowPassFilterBase(); + ~LowPassFilter(); /*! - * \brief Configure the LowPassFilterBase (access and process params). + * \brief Configure the LowPassFilter (access and process params). */ bool configure(); @@ -122,7 +122,7 @@ class LowPassFilterBase protected: /*! * \brief Internal computation of the feedforward and feedbackward coefficients - * according to the LowPassFilterBase parameters. + * according to the LowPassFilter parameters. */ void compute_internal_params() { @@ -139,23 +139,23 @@ class LowPassFilterBase /** internal data storage (wrench). */ Eigen::Matrix msg_filtered, msg_filtered_old, msg_old; double sampling_frequency, damping_frequency, damping_intensity; - double a1_; /**< feedbackward coefficient. */ - double b1_; /**< feedforward coefficient. */ + double a1_; /** feedbackward coefficient. */ + double b1_; /** feedforward coefficient. */ bool configured_ = false; }; template -LowPassFilterBase::LowPassFilterBase() : a1_(1.0), b1_(0.0) +LowPassFilter::LowPassFilter() : a1_(1.0), b1_(0.0) { } template -LowPassFilterBase::~LowPassFilterBase() +LowPassFilter::~LowPassFilter() { } template -bool LowPassFilterBase::configure() +bool LowPassFilter::configure() { compute_internal_params(); @@ -171,7 +171,7 @@ bool LowPassFilterBase::configure() } template <> -inline bool LowPassFilterBase::update( +inline bool LowPassFilter::update( const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) { if (!configured_) @@ -204,7 +204,7 @@ inline bool LowPassFilterBase::update( } template -bool LowPassFilterBase::update(const T & data_in, T & data_out) +bool LowPassFilter::update(const T & data_in, T & data_out) { if (!configured_) { @@ -219,6 +219,6 @@ bool LowPassFilterBase::update(const T & data_in, T & data_out) return true; } -} // namespace control_filters +} // namespace control_toolbox -#endif // CONTROL_FILTERS__LOW_PASS_FILTER_BASE_HPP_ +#endif // CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ From b85a21d74e147c266c45b6c30e525fc6b77c6ae9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 4 Nov 2024 22:15:23 +0000 Subject: [PATCH 6/7] Fix wrong path in deprecation warning --- include/control_filters/low_pass_filter.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index 68d12611..f969a4ea 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -22,10 +22,10 @@ // Deprecation notice #ifdef _WIN32 #pragma message( \ - "This header include is deprecated. Please update your code to use 'control_toolbox/low_pass_filter_ros.hpp' header.") //NOLINT + "This header include is deprecated. Please update your code to use 'low_pass_filter_ros.hpp' header.") //NOLINT #else #warning \ - "This header include is deprecated. Please update your code to use 'control_toolbox/low_pass_filter_ros.hpp' header." //NOLINT + "This header include is deprecated. Please update your code to use 'low_pass_filter_ros.hpp' header." //NOLINT #endif namespace control_filters From 380aa1ffd82444129bfc97d6a4e7c5eea8b4df22 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 5 Nov 2024 18:43:13 +0000 Subject: [PATCH 7/7] Use the old class/file name and move the new one to control_toolbox namespace --- CMakeLists.txt | 24 +-- control_filters.xml | 6 +- include/control_filters/low_pass_filter.hpp | 176 +++++++++++++++- .../control_filters/low_pass_filter_ros.hpp | 191 ------------------ ...ass_filter_ros.cpp => low_pass_filter.cpp} | 6 +- ..._ros.cpp => test_load_low_pass_filter.cpp} | 2 +- ...ilter_ros.cpp => test_low_pass_filter.cpp} | 10 +- ...ilter_ros.hpp => test_low_pass_filter.hpp} | 8 +- 8 files changed, 193 insertions(+), 230 deletions(-) delete mode 100644 include/control_filters/low_pass_filter_ros.hpp rename src/control_filters/{low_pass_filter_ros.cpp => low_pass_filter.cpp} (77%) rename test/control_filters/{test_load_low_pass_filter_ros.cpp => test_load_low_pass_filter.cpp} (97%) rename test/control_filters/{test_low_pass_filter_ros.cpp => test_low_pass_filter.cpp} (93%) rename test/control_filters/{test_low_pass_filter_ros.hpp => test_low_pass_filter.hpp} (88%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 208cbd1b..9786821c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,20 +62,20 @@ generate_parameter_library( src/control_filters/low_pass_filter_parameters.yaml ) -add_library(low_pass_filter_ros SHARED - src/control_filters/low_pass_filter_ros.cpp +add_library(low_pass_filter SHARED + src/control_filters/low_pass_filter.cpp ) -target_compile_features(low_pass_filter_ros PUBLIC cxx_std_17) -target_include_directories(low_pass_filter_ros PUBLIC +target_compile_features(low_pass_filter PUBLIC cxx_std_17) +target_include_directories(low_pass_filter PUBLIC $ $ $ ) -target_link_libraries(low_pass_filter_ros PUBLIC +target_link_libraries(low_pass_filter PUBLIC low_pass_filter_parameters Eigen3::Eigen ) -ament_target_dependencies(low_pass_filter_ros PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) +ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) # Install pluginlib xml pluginlib_export_plugin_description_file(filters control_filters.xml) @@ -99,14 +99,14 @@ if(BUILD_TESTING) ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) ## Control Filters - add_rostest_with_parameters_gmock(test_low_pass_filter_ros test/control_filters/test_low_pass_filter_ros.cpp + add_rostest_with_parameters_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_low_pass_filter_parameters.yaml ) - target_link_libraries(test_low_pass_filter_ros low_pass_filter_ros low_pass_filter_parameters) - ament_target_dependencies(test_low_pass_filter_ros ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + target_link_libraries(test_low_pass_filter low_pass_filter low_pass_filter_parameters) + ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS}) - ament_add_gmock(test_load_low_pass_filter test/control_filters/test_load_low_pass_filter_ros.cpp) - target_link_libraries(test_load_low_pass_filter low_pass_filter_ros low_pass_filter_parameters) + 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}) endif() @@ -115,7 +115,7 @@ install( DIRECTORY include/ DESTINATION include/control_toolbox ) -install(TARGETS control_toolbox low_pass_filter_ros low_pass_filter_parameters +install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/control_filters.xml b/control_filters.xml index dca303f8..a2280008 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,14 +1,14 @@ - + This is a low pass filter working with a double value. This is a low pass filter working with geometry_msgs::WrenchStamped. diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index f969a4ea..830dbb0d 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -15,23 +15,177 @@ #ifndef CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ #define CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ -#include "low_pass_filter_ros.hpp" +#include +#include +#include +#include +#include -// TODO(christophfroehlich) file is only for backwards compatibility, remove this file in the future +#include "filters/filter_base.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" -// Deprecation notice -#ifdef _WIN32 -#pragma message( \ - "This header include is deprecated. Please update your code to use 'low_pass_filter_ros.hpp' header.") //NOLINT -#else -#warning \ - "This header include is deprecated. Please update your code to use 'low_pass_filter_ros.hpp' header." //NOLINT -#endif +#include "control_toolbox/low_pass_filter.hpp" +#include "low_pass_filter_parameters.hpp" namespace control_filters { + +/***************************************************/ +/*! \class LowPassFilter + \brief A Low-pass filter class. + + This class implements a low-pass filter for + various data types based on an Infinite Impulse Response Filter. + For vector elements, the filtering is applied separately on + each element of the vector. + + In particular, this class implements a simplified version of + an IIR filter equation : + + \f$y(n) = b x(n-1) + a y(n-1)\f$ + + where:
+
    +
  • \f$ x(n)\f$ is the input signal +
  • \f$ y(n)\f$ is the output signal (filtered) +
  • \f$ b \f$ is the feedforward filter coefficient +
  • \f$ a \f$ is the feedback filter coefficient +
+ + and the Low-Pass coefficient equation: +
+
    +
  • \f$ a = e^{\frac{-1}{sf} \frac{2\pi df}{10^{\frac{di}{-10}}}} \f$ +
  • \f$ b = 1 - a \f$ +
+ + where:
+
    +
  • \f$ sf \f$ is the sampling frequency +
  • \f$ df \f$ is the damping frequency +
  • \f$ di \f$ is the damping intensity (amplitude) +
+ + \section Usage + + The LowPassFilter 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 -using LowPassFilter = control_filters::LowPassFilterRos; +class LowPassFilter : public filters::FilterBase +{ +public: + /*! + * \brief Configure the LowPassFilter (access and process params). + */ + bool configure() override; + + /*! + * \brief Applies one iteration of the IIR 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_; + low_pass_filter::Params parameters_; + std::shared_ptr> lpf_; +}; + +template +bool LowPassFilter::configure() +{ + clock_ = std::make_shared(RCL_SYSTEM_TIME); + 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_), "LowPass filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + catch (rclcpp::exceptions::InvalidParameterValueException & ex) { + RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + } + parameters_ = parameter_handler_->get_params(); + lpf_ = std::make_shared>( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); + + return lpf_->configure(); +} + +template <> +inline bool LowPassFilter::update( + const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) +{ + if (!this->configured_ || !lpf_ || !lpf_->is_configured()) + { + if (logger_) + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); + return false; + } + + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + lpf_->set_params( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); + } + + return lpf_->update(data_in, data_out); +} + +template +bool LowPassFilter::update(const T & data_in, T & data_out) +{ + if (!this->configured_ || !lpf_ || !lpf_->is_configured()) + { + if (logger_) + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); + return false; + } + + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + lpf_->set_params( + parameters_.sampling_frequency, + parameters_.damping_frequency, + parameters_.damping_intensity); + } + + return lpf_->update(data_in, data_out); +} + } // namespace control_filters #endif // CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ diff --git a/include/control_filters/low_pass_filter_ros.hpp b/include/control_filters/low_pass_filter_ros.hpp deleted file mode 100644 index ceab6c2c..00000000 --- a/include/control_filters/low_pass_filter_ros.hpp +++ /dev/null @@ -1,191 +0,0 @@ -// 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__LOW_PASS_FILTER_ROS_HPP_ -#define CONTROL_FILTERS__LOW_PASS_FILTER_ROS_HPP_ - -#include -#include -#include -#include -#include - -#include "filters/filter_base.hpp" -#include "geometry_msgs/msg/wrench_stamped.hpp" - -#include "control_toolbox/low_pass_filter.hpp" -#include "low_pass_filter_parameters.hpp" - -namespace control_filters -{ - -/***************************************************/ -/*! \class LowPassFilterRos - \brief A Low-pass filter class. - - This class implements a low-pass filter for - various data types based on an Infinite Impulse Response Filter. - For vector elements, the filtering is applied separately on - each element of the vector. - - In particular, this class implements a simplified version of - an IIR filter equation : - - \f$y(n) = b x(n-1) + a y(n-1)\f$ - - where:
-
    -
  • \f$ x(n)\f$ is the input signal -
  • \f$ y(n)\f$ is the output signal (filtered) -
  • \f$ b \f$ is the feedforward filter coefficient -
  • \f$ a \f$ is the feedback filter coefficient -
- - and the Low-Pass coefficient equation: -
-
    -
  • \f$ a = e^{\frac{-1}{sf} \frac{2\pi df}{10^{\frac{di}{-10}}}} \f$ -
  • \f$ b = 1 - a \f$ -
- - where:
-
    -
  • \f$ sf \f$ is the sampling frequency -
  • \f$ df \f$ is the damping frequency -
  • \f$ di \f$ is the damping intensity (amplitude) -
- - \section Usage - - The LowPassFilterRos 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 LowPassFilterRos : public filters::FilterBase -{ -public: - /*! - * \brief Configure the LowPassFilterRos (access and process params). - */ - bool configure() override; - - /*! - * \brief Applies one iteration of the IIR 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_; - low_pass_filter::Params parameters_; - std::shared_ptr> lpf_; -}; - -template -bool LowPassFilterRos::configure() -{ - clock_ = std::make_shared(RCL_SYSTEM_TIME); - 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_), "LowPass filter cannot be configured: %s", ex.what()); - parameter_handler_.reset(); - return false; - } - catch (rclcpp::exceptions::InvalidParameterValueException & ex) { - RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what()); - parameter_handler_.reset(); - return false; - } - } - parameters_ = parameter_handler_->get_params(); - lpf_ = std::make_shared>( - parameters_.sampling_frequency, - parameters_.damping_frequency, - parameters_.damping_intensity); - - return lpf_->configure(); -} - -template <> -inline bool LowPassFilterRos::update( - const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) -{ - if (!this->configured_ || !lpf_ || !lpf_->is_configured()) - { - if (logger_) - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); - return false; - } - - // Update internal parameters if required - if (parameter_handler_->is_old(parameters_)) - { - parameters_ = parameter_handler_->get_params(); - lpf_->set_params( - parameters_.sampling_frequency, - parameters_.damping_frequency, - parameters_.damping_intensity); - } - - return lpf_->update(data_in, data_out); -} - -template -bool LowPassFilterRos::update(const T & data_in, T & data_out) -{ - if (!this->configured_ || !lpf_ || !lpf_->is_configured()) - { - if (logger_) - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); - return false; - } - - // Update internal parameters if required - if (parameter_handler_->is_old(parameters_)) - { - parameters_ = parameter_handler_->get_params(); - lpf_->set_params( - parameters_.sampling_frequency, - parameters_.damping_frequency, - parameters_.damping_intensity); - } - - return lpf_->update(data_in, data_out); -} - -} // namespace control_filters - -#endif // CONTROL_FILTERS__LOW_PASS_FILTER_ROS_HPP_ diff --git a/src/control_filters/low_pass_filter_ros.cpp b/src/control_filters/low_pass_filter.cpp similarity index 77% rename from src/control_filters/low_pass_filter_ros.cpp rename to src/control_filters/low_pass_filter.cpp index 2fcac8d4..d5063722 100644 --- a/src/control_filters/low_pass_filter_ros.cpp +++ b/src/control_filters/low_pass_filter.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "control_filters/low_pass_filter_ros.hpp" +#include "control_filters/low_pass_filter.hpp" #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilterRos, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS( - control_filters::LowPassFilterRos, + control_filters::LowPassFilter, filters::FilterBase) diff --git a/test/control_filters/test_load_low_pass_filter_ros.cpp b/test/control_filters/test_load_low_pass_filter.cpp similarity index 97% rename from test/control_filters/test_load_low_pass_filter_ros.cpp rename to test/control_filters/test_load_low_pass_filter.cpp index fab3667a..91eb3568 100644 --- a/test/control_filters/test_load_low_pass_filter_ros.cpp +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -20,7 +20,7 @@ #include "rclcpp/utilities.hpp" #include "pluginlib/class_loader.hpp" -#include "control_filters/low_pass_filter_ros.hpp" +#include "control_filters/low_pass_filter.hpp" TEST(TestLoadLowPassFilter, load_low_pass_filter_double) { diff --git a/test/control_filters/test_low_pass_filter_ros.cpp b/test/control_filters/test_low_pass_filter.cpp similarity index 93% rename from test/control_filters/test_low_pass_filter_ros.cpp rename to test/control_filters/test_low_pass_filter.cpp index 0efaf5c6..4f2cc9ce 100644 --- a/test/control_filters/test_low_pass_filter_ros.cpp +++ b/test/control_filters/test_low_pass_filter.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "test_low_pass_filter_ros.hpp" +#include "test_low_pass_filter.hpp" TEST_F(LowPassFilterTest, TestLowPassWrenchFilterAllParameters) { std::shared_ptr> filter_ = - std::make_shared>(); + std::make_shared>(); // should allow configuration and find parameters in sensor_filter_chain param namespace ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", @@ -34,7 +34,7 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterAllParameters) TEST_F(LowPassFilterTest, TestLowPassWrenchFilterMissingParameter) { std::shared_ptr> filter_ = - std::make_shared>(); + std::make_shared>(); // should deny configuration as sampling frequency is missing ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", @@ -44,7 +44,7 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterMissingParameter) TEST_F(LowPassFilterTest, TestLowPassWrenchFilterInvalidThenFixedParameter) { std::shared_ptr> filter_ = - std::make_shared>(); + std::make_shared>(); // should deny configuration as sampling frequency is invalid ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", @@ -75,7 +75,7 @@ TEST_F(LowPassFilterTest, TestLowPassFilterComputation) in.wrench.torque.x = 10.0; std::shared_ptr> filter_ = - std::make_shared>(); + std::make_shared>(); // not yet configured, should deny update ASSERT_FALSE(filter_->update(in, out)); diff --git a/test/control_filters/test_low_pass_filter_ros.hpp b/test/control_filters/test_low_pass_filter.hpp similarity index 88% rename from test/control_filters/test_low_pass_filter_ros.hpp rename to test/control_filters/test_low_pass_filter.hpp index f1dcae0a..c81e495c 100644 --- a/test/control_filters/test_low_pass_filter_ros.hpp +++ b/test/control_filters/test_low_pass_filter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_FILTERS__TEST_LOW_PASS_FILTER_ROS_HPP_ -#define CONTROL_FILTERS__TEST_LOW_PASS_FILTER_ROS_HPP_ +#ifndef CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ +#define CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ #include #include #include "gmock/gmock.h" -#include "control_filters/low_pass_filter_ros.hpp" +#include "control_filters/low_pass_filter.hpp" #include "geometry_msgs/msg/wrench_stamped.hpp" #include "rclcpp/rclcpp.hpp" @@ -60,4 +60,4 @@ class LowPassFilterTest : public ::testing::Test std::thread executor_thread_; }; -#endif // CONTROL_FILTERS__TEST_LOW_PASS_FILTER_ROS_HPP_ +#endif // CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_