From 646838f9d147bb1c8b1f8a540b21e3bcc8ca86d6 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Wed, 6 Nov 2024 20:31:38 +0000
Subject: [PATCH] Throw if update is called without calling configure()

---
 include/control_filters/low_pass_filter.hpp   |  8 ++-----
 include/control_toolbox/low_pass_filter.hpp   |  6 ++++--
 test/control_filters/test_low_pass_filter.cpp | 21 +++++++++++++++----
 .../test_low_pass_filter_parameters.yaml      |  2 +-
 4 files changed, 24 insertions(+), 13 deletions(-)

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<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
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 <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
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<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
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