diff --git a/CMakeLists.txt b/CMakeLists.txt index fda0ecb7..7d3a2c73 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,46 @@ target_include_directories(control_toolbox PUBLIC ament_target_dependencies(control_toolbox PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY") + +######################## +# Build control filters +######################## +set(CONTROL_FILTERS_INCLUDE_DEPENDS + filters + rclcpp + generate_parameter_library + pluginlib + geometry_msgs + Eigen3 +) + +foreach(Dependency IN ITEMS ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library( + low_pass_filter_parameters + src/control_filters/low_pass_filter_parameters.yaml +) + +add_library(low_pass_filter SHARED + src/control_filters/low_pass_filter.cpp +) +target_compile_features(low_pass_filter PUBLIC cxx_std_17) +target_include_directories(low_pass_filter PUBLIC + $ + $ + $ +) +target_link_libraries(low_pass_filter PUBLIC low_pass_filter_parameters) +ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + +# Install pluginlib xml +pluginlib_export_plugin_description_file(filters control_filters.xml) + +########## +# Testing +########## if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ament_cmake_gtest REQUIRED) @@ -55,13 +95,25 @@ if(BUILD_TESTING) ament_add_gtest(pid_publisher_tests test/pid_publisher_tests.cpp) target_link_libraries(pid_publisher_tests control_toolbox) 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 + ${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}) + + 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() +# Install install( DIRECTORY include/ DESTINATION include/control_toolbox ) -install(TARGETS control_toolbox +install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -69,5 +121,5 @@ install(TARGETS control_toolbox ) ament_export_targets(export_control_toolbox HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} ${CONTROL_FILTERS_INCLUDE_DEPENDS}) ament_package() diff --git a/control_filters.xml b/control_filters.xml new file mode 100644 index 00000000..a2280008 --- /dev/null +++ b/control_filters.xml @@ -0,0 +1,18 @@ + + + + + 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 new file mode 100644 index 00000000..e4a11782 --- /dev/null +++ b/include/control_filters/low_pass_filter.hpp @@ -0,0 +1,252 @@ +// 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 +#include +#include +#include +#include + +#include "low_pass_filter_parameters.hpp" +#include "filters/filter_base.hpp" + +#include "geometry_msgs/msg/wrench_stamped.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 +class LowPassFilter : public filters::FilterBase +{ +public: + // Default constructor + LowPassFilter(); + + /*! + * \brief Destructor of LowPassFilter class. + */ + ~LowPassFilter() override; + + /*! + * \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; + +protected: + /*! + * \brief Internal computation of the feedforward and feedbackward coefficients + * according to the LowPassFilter parameters. + */ + 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))); + 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 a1_; /**< feedbackward coefficient. */ + double b1_; /**< feedforward coefficient. */ +}; + +template +LowPassFilter::LowPassFilter() : a1_(1.0), b1_(0.0) +{ +} + +template +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 + filtered_value = filtered_old_value = old_value = 0; + // TODO(destogl): make the size parameterizable and more intelligent is using complex types + for (size_t i = 0; i < 6; ++i) + { + msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0; + } + + return true; +} + +template <> +inline bool LowPassFilter::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(); + compute_internal_params(); + } + + // IIR Filter + msg_filtered = b1_ * msg_old + a1_ * msg_filtered_old; + msg_filtered_old = msg_filtered; + + // TODO(destogl): use wrenchMsgToEigen + msg_old[0] = data_in.wrench.force.x; + msg_old[1] = data_in.wrench.force.y; + msg_old[2] = data_in.wrench.force.z; + msg_old[3] = data_in.wrench.torque.x; + msg_old[4] = data_in.wrench.torque.y; + msg_old[5] = data_in.wrench.torque.z; + + data_out.wrench.force.x = msg_filtered[0]; + data_out.wrench.force.y = msg_filtered[1]; + data_out.wrench.force.z = msg_filtered[2]; + data_out.wrench.torque.x = msg_filtered[3]; + data_out.wrench.torque.y = msg_filtered[4]; + data_out.wrench.torque.z = msg_filtered[5]; + + // copy the header + data_out.header = data_in.header; + return true; +} + +template +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; + old_value = data_in; + + return true; +} + +} // namespace control_filters + +#endif // CONTROL_FILTERS__LOW_PASS_FILTER_HPP_ diff --git a/package.xml b/package.xml index f7290ed4..7bbdec0f 100644 --- a/package.xml +++ b/package.xml @@ -19,6 +19,10 @@ ament_cmake control_msgs + filters + geometry_msgs + pluginlib + generate_parameter_library rclcpp rcutils realtime_tools diff --git a/src/control_filters/README.md b/src/control_filters/README.md new file mode 100644 index 00000000..cac37a26 --- /dev/null +++ b/src/control_filters/README.md @@ -0,0 +1,31 @@ +# Control 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). + +## Low Pass filter + +This filter implements a low-pass filter in the form of an [IIR filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), applied to a `data_in` (double or wrench). +The feedforward and feedback coefficients of the IIR filter are computed from the low-pass filter parameters. + +### Required parameters + +* sampling frequency as `sf` +* damping frequency as `df` +* damping intensity as `di` + +### Algorithm + +Given +* above-required parameters, `sf`, `df`, `di` +* `data_in`, a double or wrench `x` + +Compute `data_out`, the filtered output `y(n)` with equation: + +y(n) = b x(n-1) + a y(n-1) + +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 diff --git a/src/control_filters/low_pass_filter.cpp b/src/control_filters/low_pass_filter.cpp new file mode 100644 index 00000000..d5063722 --- /dev/null +++ b/src/control_filters/low_pass_filter.cpp @@ -0,0 +1,23 @@ +// 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 "control_filters/low_pass_filter.hpp" + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter, filters::FilterBase) + +PLUGINLIB_EXPORT_CLASS( + control_filters::LowPassFilter, + filters::FilterBase) diff --git a/src/control_filters/low_pass_filter_parameters.yaml b/src/control_filters/low_pass_filter_parameters.yaml new file mode 100644 index 00000000..bb26f5b7 --- /dev/null +++ b/src/control_filters/low_pass_filter_parameters.yaml @@ -0,0 +1,19 @@ +low_pass_filter: + sampling_frequency: { + type: double, + description: "Sampling frequency", + validation: { + gt<>: [0.0] + }, + } + damping_frequency: { + type: double, + description: "Damping frequency", + validation: { + gt<>: [0.0] + }, + } + damping_intensity: { + type: double, + description: "Damping intensity", + } diff --git a/test/control_filters/test_load_low_pass_filter.cpp b/test/control_filters/test_load_low_pass_filter.cpp new file mode 100644 index 00000000..f3c00fa8 --- /dev/null +++ b/test/control_filters/test_load_low_pass_filter.cpp @@ -0,0 +1,66 @@ +// 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 "control_filters/low_pass_filter.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/utilities.hpp" +#include + + +TEST(TestLoadLowPassFilter, load_low_pass_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/LowPassFilterDouble"; + ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); + ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + + rclcpp::shutdown(); +} + +TEST(TestLoadLowPassFilter, load_low_pass_filter_wrench) +{ + 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/LowPassFilterWrench"; + ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); + ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + + rclcpp::shutdown(); +} diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp new file mode 100644 index 00000000..4f2cc9ce --- /dev/null +++ b/test/control_filters/test_low_pass_filter.cpp @@ -0,0 +1,138 @@ +// 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_low_pass_filter.hpp" + +TEST_F(LowPassFilterTest, TestLowPassWrenchFilterAllParameters) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // should allow configuration and find parameters in sensor_filter_chain param namespace + ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // change a parameter + node_->set_parameter(rclcpp::Parameter("sampling_frequency", 500.0)); + // accept second call to configure with valid parameters to already configured filter + ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} + + +TEST_F(LowPassFilterTest, TestLowPassWrenchFilterMissingParameter) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // should deny configuration as sampling frequency is missing + ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} + +TEST_F(LowPassFilterTest, TestLowPassWrenchFilterInvalidThenFixedParameter) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // should deny configuration as sampling frequency is invalid + ASSERT_FALSE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // fix the param + node_->set_parameter(rclcpp::Parameter("sampling_frequency", 1000.0)); + // should allow configuration and pass second call to unconfigured filter + ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} + +TEST_F(LowPassFilterTest, TestLowPassFilterComputation) +{ + // parameters should match the test yaml file + double sampling_freq = 1000.0; + double damping_freq = 20.5; + double damping_intensity = 1.25; + + double a1, b1; + a1 = exp(-1.0 / sampling_freq * (2.0 * M_PI * damping_freq) / + (pow(10.0, damping_intensity / -10.0))); + b1 = 1.0 - a1; + + geometry_msgs::msg::WrenchStamped in, calculated, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + std::shared_ptr> filter_ = + std::make_shared>(); + + // not yet configured, should deny update + ASSERT_FALSE(filter_->update(in, out)); + + // configure + ASSERT_TRUE(filter_->configure("", "TestLowPassFilter", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // first filter pass, output should be zero as no old wrench was stored + ASSERT_TRUE(filter_->update(in, out)); + ASSERT_EQ(out.wrench.force.x, 0.0); + ASSERT_EQ(out.wrench.force.y, 0.0); + ASSERT_EQ(out.wrench.force.z, 0.0); + ASSERT_EQ(out.wrench.torque.x, 0.0); + ASSERT_EQ(out.wrench.torque.y, 0.0); + ASSERT_EQ(out.wrench.torque.z, 0.0); + + // second filter pass with same values: + // calculate wrench by hand + calculated.wrench.force.x = b1 * in.wrench.force.x; + calculated.wrench.force.y = b1 * in.wrench.force.y; + calculated.wrench.force.z = b1 * in.wrench.force.z; + calculated.wrench.torque.x = b1 * in.wrench.torque.x; + calculated.wrench.torque.y = b1 * in.wrench.torque.y; + calculated.wrench.torque.z = b1 * in.wrench.torque.z; + // check equality with low-pass-filter + ASSERT_TRUE(filter_->update(in, out)); + ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x); + ASSERT_EQ(out.wrench.force.y, calculated.wrench.force.y); + ASSERT_EQ(out.wrench.force.z, calculated.wrench.force.z); + ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x); + ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); + ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); + + // update once again and check results + // calculate wrench by hand + calculated.wrench.force.x = b1 * in.wrench.force.x + a1 * calculated.wrench.force.x; + calculated.wrench.force.y = b1 * in.wrench.force.y + a1 * calculated.wrench.force.y; + calculated.wrench.force.z = b1 * in.wrench.force.z + a1 * calculated.wrench.force.z; + calculated.wrench.torque.x = b1 * in.wrench.torque.x + a1 * calculated.wrench.torque.x; + calculated.wrench.torque.y = b1 * in.wrench.torque.y + a1 * calculated.wrench.torque.y; + calculated.wrench.torque.z = b1 * in.wrench.torque.z + a1 * calculated.wrench.torque.z; + // check equality with low-pass-filter + ASSERT_TRUE(filter_->update(in, out)); + ASSERT_EQ(out.wrench.force.x, calculated.wrench.force.x); + ASSERT_EQ(out.wrench.force.y, calculated.wrench.force.y); + ASSERT_EQ(out.wrench.force.z, calculated.wrench.force.z); + ASSERT_EQ(out.wrench.torque.x, calculated.wrench.torque.x); + ASSERT_EQ(out.wrench.torque.y, calculated.wrench.torque.y); + ASSERT_EQ(out.wrench.torque.z, calculated.wrench.torque.z); +} + +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_low_pass_filter.hpp b/test/control_filters/test_low_pass_filter.hpp new file mode 100644 index 00000000..c81e495c --- /dev/null +++ b/test/control_filters/test_low_pass_filter.hpp @@ -0,0 +1,63 @@ +// 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_LOW_PASS_FILTER_HPP_ +#define CONTROL_FILTERS__TEST_LOW_PASS_FILTER_HPP_ + +#include +#include +#include "gmock/gmock.h" + +#include "control_filters/low_pass_filter.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_low_pass_filter"); +} // namespace + +class LowPassFilterTest : 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(); }); + } + + LowPassFilterTest() + { + 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_LOW_PASS_FILTER_HPP_ diff --git a/test/control_filters/test_low_pass_filter_parameters.yaml b/test/control_filters/test_low_pass_filter_parameters.yaml new file mode 100644 index 00000000..11e08cc1 --- /dev/null +++ b/test/control_filters/test_low_pass_filter_parameters.yaml @@ -0,0 +1,22 @@ +TestLowPassWrenchFilterAllParameters: + ros__parameters: + sampling_frequency: 1000.0 + damping_frequency: 20.5 + damping_intensity: 1.25 + +TestLowPassWrenchFilterMissingParameter: + ros__parameters: + damping_frequency: 20.5 + damping_intensity: 1.25 + +TestLowPassWrenchFilterInvalidThenFixedParameter: + ros__parameters: + sampling_frequency: 0.0 + damping_frequency: 20.5 + damping_intensity: 1.25 + +TestLowPassFilterComputation: + ros__parameters: + sampling_frequency: 1000.0 + damping_frequency: 20.5 + damping_intensity: 1.25