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 8c8acded..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 @@ -159,7 +161,7 @@ inline bool LowPassFilter::update( { if (!configured_) { - return false; + throw std::runtime_error("Filter is not configured"); } // IIR Filter @@ -191,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