Throw if update is called without calling configure()

ros-controls · Nov 6, 2024 · 646838f · 646838f
1 parent 683ffe9
commit 646838f
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 13 deletions.
diff --git a/include/control_filters/low_pass_filter.hpp b/include/control_filters/low_pass_filter.hpp
@@ -145,9 +145,7 @@ inline bool LowPassFilter<geometry_msgs::msg::WrenchStamped>::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<T>::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
@@ -16,8 +16,10 @@
 #define CONTROL_TOOLBOX__LOW_PASS_FILTER_HPP_
 
 #include <Eigen/Dense>
+
 #include <cmath>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -159,7 +161,7 @@ inline bool LowPassFilter<geometry_msgs::msg::WrenchStamped>::update(
 {
   if (!configured_)
   {
-    return false;
+    throw std::runtime_error("Filter is not configured");
   }
 
   // IIR Filter
@@ -191,7 +193,7 @@ bool LowPassFilter<T>::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
@@ -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<filters::FilterBase<double>> filter_ =
+        std::make_shared<control_filters::LowPassFilter<double>>();
+    double in, out;
+    ASSERT_THROW(filter_->update(in, out), std::runtime_error);
+}
+
+TEST_F(LowPassFilterTest, TestLowPassWrenchFilterThrowsUnconfigured)
+{
+    std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
+        std::make_shared<control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>>();
+    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<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
         std::make_shared<control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>>();
 
-    // 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
@@ -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