Add standalone version of LPF (#164)

control_filters.xml

@@ -1,14 +1,14 @@
 <class_libraries>
     <library path="low_pass_filter">
       <class name="control_filters/LowPassFilterDouble"
-             type="control_filters::LowPassFilter&lt;double&gt;"
+             type="control_filters::LowPassFilterRos&lt;double&gt;"
              base_class_type="filters::FilterBase&lt;double&gt;">
         <description>
           This is a low pass filter working with a double value.
         </description>
       </class>
       <class name="control_filters/LowPassFilterWrench"
-             type="control_filters::LowPassFilter&lt;geometry_msgs::msg::WrenchStamped&gt;"
+             type="control_filters::LowPassFilterRos&lt;geometry_msgs::msg::WrenchStamped&gt;"
              base_class_type="filters::FilterBase&lt;geometry_msgs::msg::WrenchStamped&gt;">
         <description>
             This is a low pass filter working with geometry_msgs::WrenchStamped.

include/control_filters/low_pass_filter.hpp

@@ -21,7 +21,6 @@
 #include <string>
 #include <vector>
 
-#include "low_pass_filter_parameters.hpp"
 #include "filters/filter_base.hpp"
 
 #include "geometry_msgs/msg/wrench_stamped.hpp"
@@ -82,6 +81,10 @@ class LowPassFilter : public filters::FilterBase<T>
   // Default constructor
   LowPassFilter();
 
+  LowPassFilter(double sampling_frequency, double damping_frequency, double damping_intensity){
+    set_params(sampling_frequency, damping_frequency, damping_intensity);
+  }
+
   /*!
    * \brief Destructor of LowPassFilter class.
    */
@@ -102,6 +105,19 @@ class LowPassFilter : public filters::FilterBase<T>
    */
   bool update(const T & data_in, T & data_out) override;
 
+  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;
+  }
+
 protected:
   /*!
    * \brief Internal computation of the feedforward and feedbackward coefficients
@@ -110,22 +126,18 @@ class LowPassFilter : public filters::FilterBase<T>
   void compute_internal_params()
   {
     a1_ = exp(
-      -1.0 / parameters_.sampling_frequency * (2.0 * M_PI * parameters_.damping_frequency) /
-      (pow(10.0, parameters_.damping_intensity / -10.0)));
+      -1.0 / sampling_frequency * (2.0 * M_PI * damping_frequency) /
+      (pow(10.0, damping_intensity / -10.0)));
     b1_ = 1.0 - a1_;
   };
 
 private:
-  rclcpp::Clock::SharedPtr clock_;
-  std::shared_ptr<rclcpp::Logger> logger_;
-  std::shared_ptr<low_pass_filter::ParamListener> parameter_handler_;
-  low_pass_filter::Params parameters_;
-
   // Filter parameters
   /** 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. */
 };
@@ -143,31 +155,6 @@ LowPassFilter<T>::~LowPassFilter()
 template <typename T>
 bool LowPassFilter<T>::configure()
 {
-  clock_ = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
-  logger_.reset(
-    new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_)));
-
-  // Initialize the parameters once
-  if (!parameter_handler_)
-  {
-    try
-    {
-      parameter_handler_ =
-        std::make_shared<low_pass_filter::ParamListener>(this->params_interface_,
-                                                         this->param_prefix_);
-    }
-    catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
-      RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what());
-      parameter_handler_.reset();
-      return false;
-    }
-    catch (rclcpp::exceptions::InvalidParameterValueException & ex)  {
-      RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what());
-      parameter_handler_.reset();
-      return false;
-    }
-  }
-  parameters_ = parameter_handler_->get_params();
   compute_internal_params();
 
   // Initialize storage Vectors
@@ -187,18 +174,9 @@ inline bool LowPassFilter<geometry_msgs::msg::WrenchStamped>::update(
 {
   if (!this->configured_)
   {
-    if (logger_)
-      RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured");
     return false;
   }
 
-  // Update internal parameters if required
-  if (parameter_handler_->is_old(parameters_))
-  {
-    parameters_ = parameter_handler_->get_params();
-    compute_internal_params();
-  }
-
   // IIR Filter
   msg_filtered = b1_ * msg_old + a1_ * msg_filtered_old;
   msg_filtered_old = msg_filtered;
@@ -228,17 +206,9 @@ bool LowPassFilter<T>::update(const T & data_in, T & data_out)
 {
   if (!this->configured_)
   {
-    RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured");
     return false;
   }
 
-  // Update internal parameters if required
-  if (parameter_handler_->is_old(parameters_))
-  {
-    parameters_ = parameter_handler_->get_params();
-    compute_internal_params();
-  }
-
   // Filter
   data_out = b1_ * old_value + a1_ * filtered_old_value;
   filtered_old_value = data_out;

include/control_filters/low_pass_filter_ros.hpp

@@ -0,0 +1,192 @@
+// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CONTROL_FILTERS__LOW_PASS_FILTER_ROS_HPP_
+#define CONTROL_FILTERS__LOW_PASS_FILTER_ROS_HPP_
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "low_pass_filter.hpp"
+#include "low_pass_filter_parameters.hpp"
+
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+
+namespace control_filters
+{
+
+/***************************************************/
+/*! \class LowPassFilterRos
+  \brief A Low-pass filter class.
+
+  This class implements a low-pass filter for
+  various data types based on an Infinite Impulse Response Filter.
+  For vector elements, the filtering is applied separately on
+  each element of the vector.
+
+  In particular, this class implements a simplified version of
+  an IIR filter equation :
+
+  \f$y(n) = b x(n-1) + a y(n-1)\f$
+
+  where: <br>
+  <UL TYPE="none">
+  <LI>  \f$ x(n)\f$ is the input signal
+  <LI>  \f$ y(n)\f$ is the output signal (filtered)
+  <LI>  \f$ b \f$ is the feedforward filter coefficient
+  <LI>  \f$ a \f$ is the feedback filter coefficient
+  </UL>
+
+  and the Low-Pass coefficient equation:
+  <br>
+  <UL TYPE="none">
+  <LI>  \f$ a = e^{\frac{-1}{sf} \frac{2\pi df}{10^{\frac{di}{-10}}}} \f$
+  <LI>  \f$ b = 1 - a \f$
+  </UL>
+
+    where: <br>
+  <UL TYPE="none">
+  <LI>  \f$ sf \f$ is the sampling frequency
+  <LI>  \f$ df \f$ is the damping frequency
+  <LI>  \f$ di \f$ is the damping intensity (amplitude)
+  </UL>
+
+  \section Usage
+
+  The LowPassFilterRos class is meant to be instantiated as a filter in
+  a controller but can also be used elsewhere.
+  For manual instantiation, you should first call configure()
+  (in non-realtime) and then call update() at every update step.
+
+*/
+/***************************************************/
+
+template <typename T>
+class LowPassFilterRos : public LowPassFilter<T>
+{
+public:
+  /*!
+   * \brief Configure the LowPassFilterRos (access and process params).
+   */
+  bool configure() override;
+
+  /*!
+   * \brief Applies one iteration of the IIR filter.
+   *
+   * \param data_in input to the filter
+   * \param data_out filtered output
+   *
+   * \returns false if filter is not configured, true otherwise
+   */
+  bool update(const T & data_in, T & data_out) override;
+
+private:
+  rclcpp::Clock::SharedPtr clock_;
+  std::shared_ptr<rclcpp::Logger> logger_;
+  std::shared_ptr<low_pass_filter::ParamListener> parameter_handler_;
+  low_pass_filter::Params parameters_;
+};
+
+template <typename T>
+bool LowPassFilterRos<T>::configure()
+{
+  clock_ = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+  logger_.reset(
+    new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_)));
+
+  // Initialize the parameters once
+  if (!parameter_handler_)
+  {
+    try
+    {
+      parameter_handler_ =
+        std::make_shared<low_pass_filter::ParamListener>(this->params_interface_,
+                                                         this->param_prefix_);
+    }
+    catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+      RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what());
+      parameter_handler_.reset();
+      return false;
+    }
+    catch (rclcpp::exceptions::InvalidParameterValueException & ex)  {
+      RCLCPP_ERROR((*logger_), "LowPass filter cannot be configured: %s", ex.what());
+      parameter_handler_.reset();
+      return false;
+    }
+  }
+  parameters_ = parameter_handler_->get_params();
+  LowPassFilter<T>::set_params(
+    parameters_.sampling_frequency,
+    parameters_.damping_frequency,
+    parameters_.damping_intensity);
+  LowPassFilter<T>::compute_internal_params();
+
+  return LowPassFilter<T>::configure();
+}
+
+template <>
+inline bool LowPassFilterRos<geometry_msgs::msg::WrenchStamped>::update(
+  const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out)
+{
+  if (!this->configured_)
+  {
+    if (logger_)
+      RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured");
+    return false;
+  }
+
+  // Update internal parameters if required
+  if (parameter_handler_->is_old(parameters_))
+  {
+    parameters_ = parameter_handler_->get_params();
+    LowPassFilter<geometry_msgs::msg::WrenchStamped>::set_params(
+      parameters_.sampling_frequency,
+      parameters_.damping_frequency,
+      parameters_.damping_intensity);
+    LowPassFilter<geometry_msgs::msg::WrenchStamped>::compute_internal_params();
+  }
+
+  return LowPassFilter<geometry_msgs::msg::WrenchStamped>::update(data_in, data_out);
+}
+
+template <typename T>
+bool LowPassFilterRos<T>::update(const T & data_in, T & data_out)
+{
+  if (!this->configured_)
+  {
+    if (logger_)
+      RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured");
+    return false;
+  }
+
+  // Update internal parameters if required
+  if (parameter_handler_->is_old(parameters_))
+  {
+    parameters_ = parameter_handler_->get_params();
+    LowPassFilter<T>::set_params(
+      parameters_.sampling_frequency,
+      parameters_.damping_frequency,
+      parameters_.damping_intensity);
+    LowPassFilter<T>::compute_internal_params();
+  }
+
+  return LowPassFilter<T>::update(data_in, data_out);
+}
+
+}  // namespace control_filters
+
+#endif  // CONTROL_FILTERS__LOW_PASS_FILTER_ROS_HPP_

src/control_filters/low_pass_filter.cpp

@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "control_filters/low_pass_filter.hpp"
+#include "control_filters/low_pass_filter_ros.hpp"
 
 #include "pluginlib/class_list_macros.hpp"
 
-PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilter<double>, filters::FilterBase<double>)
+PLUGINLIB_EXPORT_CLASS(control_filters::LowPassFilterRos<double>, filters::FilterBase<double>)
 
 PLUGINLIB_EXPORT_CLASS(
-  control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>,
+  control_filters::LowPassFilterRos<geometry_msgs::msg::WrenchStamped>,
   filters::FilterBase<geometry_msgs::msg::WrenchStamped>)

test/control_filters/test_low_pass_filter.cpp

@@ -17,7 +17,7 @@
 TEST_F(LowPassFilterTest, TestLowPassWrenchFilterAllParameters)
 {
     std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
-        std::make_shared<control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>>();
+        std::make_shared<control_filters::LowPassFilterRos<geometry_msgs::msg::WrenchStamped>>();
 
     // should allow configuration and find parameters in sensor_filter_chain param namespace
     ASSERT_TRUE(filter_->configure("", "TestLowPassFilter",
@@ -34,7 +34,7 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterAllParameters)
 TEST_F(LowPassFilterTest, TestLowPassWrenchFilterMissingParameter)
 {
     std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
-        std::make_shared<control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>>();
+        std::make_shared<control_filters::LowPassFilterRos<geometry_msgs::msg::WrenchStamped>>();
 
     // should deny configuration as sampling frequency is missing
     ASSERT_FALSE(filter_->configure("", "TestLowPassFilter",
@@ -44,7 +44,7 @@ TEST_F(LowPassFilterTest, TestLowPassWrenchFilterMissingParameter)
 TEST_F(LowPassFilterTest, TestLowPassWrenchFilterInvalidThenFixedParameter)
 {
     std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
-        std::make_shared<control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>>();
+        std::make_shared<control_filters::LowPassFilterRos<geometry_msgs::msg::WrenchStamped>>();
 
     // should deny configuration as sampling frequency is invalid
     ASSERT_FALSE(filter_->configure("", "TestLowPassFilter",
@@ -75,7 +75,7 @@ TEST_F(LowPassFilterTest, TestLowPassFilterComputation)
     in.wrench.torque.x = 10.0;
 
     std::shared_ptr<filters::FilterBase<geometry_msgs::msg::WrenchStamped>> filter_ =
-        std::make_shared<control_filters::LowPassFilter<geometry_msgs::msg::WrenchStamped>>();
+        std::make_shared<control_filters::LowPassFilterRos<geometry_msgs::msg::WrenchStamped>>();
 
     // not yet configured, should deny update
     ASSERT_FALSE(filter_->update(in, out));