diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp index 830dbb0d..4601076c 100644 --- a/include/control_filters/low_pass_filter.hpp +++ b/include/control_filters/low_pass_filter.hpp @@ -145,9 +145,7 @@ inline bool LowPassFilter::update( { if (!this->configured_ || !lpf_ || !lpf_->is_configured()) { - if (logger_) - RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); - return false; + throw std::runtime_error("Filter is not configured"); } // Update internal parameters if required @@ -168,9 +166,7 @@ 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; + throw std::runtime_error("Filter is not configured"); } // Update internal parameters if required diff --git a/include/control_toolbox/low_pass_filter.hpp b/include/control_toolbox/low_pass_filter.hpp index df214d09..07351bdd 100644 --- a/include/control_toolbox/low_pass_filter.hpp +++ b/include/control_toolbox/low_pass_filter.hpp @@ -16,8 +16,10 @@ #define CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_ #include + #include #include +#include #include #include @@ -101,30 +103,16 @@ class LowPassFilter */ bool update(const T & data_in, T & data_out); - 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; - } - bool is_configured() const { return configured_; } -protected: /*! * \brief Internal computation of the feedforward and feedbackward coefficients * according to the LowPassFilter parameters. */ - void compute_internal_params() + void set_params(double sampling_frequency, double damping_frequency, double damping_intensity) { a1_ = exp( -1.0 / sampling_frequency * (2.0 * M_PI * damping_frequency) / @@ -134,13 +122,12 @@ class LowPassFilter private: // Filter parameters + double a1_; /** feedbackward coefficient. */ + double b1_; /** feedforward coefficient. */ /** 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. */ bool configured_ = false; }; @@ -157,8 +144,6 @@ LowPassFilter::~LowPassFilter() template bool LowPassFilter::configure() { - 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 @@ -176,7 +161,7 @@ inline bool LowPassFilter::update( { if (!configured_) { - return false; + throw std::runtime_error("Filter is not configured"); } // IIR Filter @@ -208,7 +193,7 @@ bool LowPassFilter::update(const T & data_in, T & data_out) { if (!configured_) { - return false; + throw std::runtime_error("Filter is not configured"); } // Filter diff --git a/test/control_filters/test_low_pass_filter.cpp b/test/control_filters/test_low_pass_filter.cpp index 4f2cc9ce..fcb76ca0 100644 --- a/test/control_filters/test_low_pass_filter.cpp +++ b/test/control_filters/test_low_pass_filter.cpp @@ -57,7 +57,23 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterInvalidThenFixedParameter) node_->get_node_logging_interface(), node_->get_node_parameters_interface())); } -TEST_F(LowPassFilterTest, TestLowPassFilterComputation) +TEST_F(LowPassFilterTest, TestLowPassFilterThrowsUnconfigured) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + double in, out; + ASSERT_THROW(filter_->update(in, out), std::runtime_error); +} + +TEST_F(LowPassFilterTest, TestLowPassWrenchFilterThrowsUnconfigured) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + geometry_msgs::msg::WrenchStamped in, out; + ASSERT_THROW(filter_->update(in, out), std::runtime_error); +} + +TEST_F(LowPassFilterTest, TestLowPassWrenchFilterComputation) { // parameters should match the test yaml file double sampling_freq = 1000.0; @@ -77,9 +93,6 @@ TEST_F(LowPassFilterTest, TestLowPassFilterComputation) 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())); diff --git a/test/control_filters/test_low_pass_filter_parameters.yaml b/test/control_filters/test_low_pass_filter_parameters.yaml index 11e08cc1..d220da10 100644 --- a/test/control_filters/test_low_pass_filter_parameters.yaml +++ b/test/control_filters/test_low_pass_filter_parameters.yaml @@ -15,7 +15,7 @@ TestLowPassWrenchFilterInvalidThenFixedParameter: damping_frequency: 20.5 damping_intensity: 1.25 -TestLowPassFilterComputation: +TestLowPassWrenchFilterComputation: ros__parameters: sampling_frequency: 1000.0 damping_frequency: 20.5