LPF: Throw if calling udpate unconfigured (#229)

ros-controls · Nov 6, 2024 · 795b9e6 · 795b9e6
1 parent 7d008a4
commit 795b9e6
Show file tree

Hide file tree

Showing 4 changed files with 27 additions and 33 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>
 
@@ -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<double, 6, 1> 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<T>::~LowPassFilter()
 template <typename T>
 bool LowPassFilter<T>::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<geometry_msgs::msg::WrenchStamped>::update(
 {
   if (!configured_)
   {
-    return false;
+    throw std::runtime_error("Filter is not configured");
   }
 
   // IIR Filter
@@ -208,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