Add lowpass filter

CMakeLists.txt

@@ -41,6 +41,46 @@ target_include_directories(control_toolbox PUBLIC
 ament_target_dependencies(control_toolbox PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY")
 
+
+########################
+# Build control filters
+########################
+set(CONTROL_FILTERS_INCLUDE_DEPENDS
+  filters
+  rclcpp
+  generate_parameter_library
+  pluginlib
+  geometry_msgs
+  Eigen3
+)
+
+foreach(Dependency IN ITEMS ${CONTROL_FILTERS_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(
+  low_pass_filter_parameters
+  src/control_filters/low_pass_filter_parameters.yaml
+)
+
+add_library(low_pass_filter SHARED
+  src/control_filters/low_pass_filter.cpp
+)
+target_compile_features(low_pass_filter PUBLIC cxx_std_17)
+target_include_directories(low_pass_filter PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
+    $<INSTALL_INTERFACE:include/control_toolbox>
+)
+target_link_libraries(low_pass_filter PUBLIC low_pass_filter_parameters)
+ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS})
+
+# Install pluginlib xml
+pluginlib_export_plugin_description_file(filters control_filters.xml)
+
+##########
+# Testing
+##########
 if(BUILD_TESTING)
   find_package(ament_cmake_gmock REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
@@ -55,19 +95,31 @@ if(BUILD_TESTING)
   ament_add_gtest(pid_publisher_tests test/pid_publisher_tests.cpp)
   target_link_libraries(pid_publisher_tests control_toolbox)
   ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle)
+
+  ## Control Filters
+  add_rostest_with_parameters_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_low_pass_filter_parameters.yaml
+  )
+  target_link_libraries(test_low_pass_filter low_pass_filter low_pass_filter_parameters)
+  ament_target_dependencies(test_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS})
+
+  ament_add_gmock(test_load_low_pass_filter test/control_filters/test_load_low_pass_filter.cpp)
+  target_link_libraries(test_load_low_pass_filter low_pass_filter low_pass_filter_parameters)
+  ament_target_dependencies(test_load_low_pass_filter ${CONTROL_FILTERS_INCLUDE_DEPENDS})
 endif()
 
+# Install
 install(
   DIRECTORY include/
   DESTINATION include/control_toolbox
 )
-install(TARGETS control_toolbox
+install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters
   EXPORT export_control_toolbox
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
 )
 
 ament_export_targets(export_control_toolbox HAS_LIBRARY_TARGET)
-ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} ${CONTROL_FILTERS_INCLUDE_DEPENDS})
 ament_package()

control_filters.xml

@@ -0,0 +1,18 @@
+<class_libraries>
+    <library path="low_pass_filter">
+      <class name="control_filters/LowPassFilterDouble"
+             type="control_filters::LowPassFilter&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;"
+             base_class_type="filters::FilterBase&lt;geometry_msgs::msg::WrenchStamped&gt;">
+        <description>
+            This is a low pass filter working with geometry_msgs::WrenchStamped.
+        </description>
+      </class>
+    </library>
+</class_libraries>

include/control_filters/low_pass_filter.hpp

@@ -0,0 +1,252 @@
+// 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_HPP_
+#define CONTROL_FILTERS__LOW_PASS_FILTER_HPP_
+
+#include <Eigen/Dense>
+#include <cmath>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "low_pass_filter_parameters.hpp"
+#include "filters/filter_base.hpp"
+
+#include "geometry_msgs/msg/wrench_stamped.hpp"
+
+namespace control_filters
+{
+
+/***************************************************/
+/*! \class LowPassFilter
+  \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 LowPassFilter 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 LowPassFilter : public filters::FilterBase<T>
+{
+public:
+  // Default constructor
+  LowPassFilter();
+
+  /*!
+   * \brief Destructor of LowPassFilter class.
+   */
+  ~LowPassFilter() override;
+
+  /*!
+   * \brief Configure the LowPassFilter (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;
+
+protected:
+  /*!
+   * \brief Internal computation of the feedforward and feedbackward coefficients
+   * according to the LowPassFilter parameters.
+   */
+  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)));
+    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 a1_; /**< feedbackward coefficient. */
+  double b1_; /**< feedforward coefficient. */
+};
+
+template <typename T>
+LowPassFilter<T>::LowPassFilter() : a1_(1.0), b1_(0.0)
+{
+}
+
+template <typename T>
+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
+  filtered_value = filtered_old_value = old_value = 0;
+  // TODO(destogl): make the size parameterizable and more intelligent is using complex types
+  for (size_t i = 0; i < 6; ++i)
+  {
+    msg_filtered[i] = msg_filtered_old[i] = msg_old[i] = 0;
+  }
+
+  return true;
+}
+
+template <>
+inline bool LowPassFilter<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();
+    compute_internal_params();
+  }
+
+  // IIR Filter
+  msg_filtered = b1_ * msg_old + a1_ * msg_filtered_old;
+  msg_filtered_old = msg_filtered;
+
+  // TODO(destogl): use wrenchMsgToEigen
+  msg_old[0] = data_in.wrench.force.x;
+  msg_old[1] = data_in.wrench.force.y;
+  msg_old[2] = data_in.wrench.force.z;
+  msg_old[3] = data_in.wrench.torque.x;
+  msg_old[4] = data_in.wrench.torque.y;
+  msg_old[5] = data_in.wrench.torque.z;
+
+  data_out.wrench.force.x = msg_filtered[0];
+  data_out.wrench.force.y = msg_filtered[1];
+  data_out.wrench.force.z = msg_filtered[2];
+  data_out.wrench.torque.x = msg_filtered[3];
+  data_out.wrench.torque.y = msg_filtered[4];
+  data_out.wrench.torque.z = msg_filtered[5];
+
+  // copy the header
+  data_out.header = data_in.header;
+  return true;
+}
+
+template <typename T>
+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;
+  old_value = data_in;
+
+  return true;
+}
+
+}  // namespace control_filters
+
+#endif  // CONTROL_FILTERS__LOW_PASS_FILTER_HPP_

package.xml

@@ -19,6 +19,10 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>control_msgs</depend>
+  <depend>filters</depend>
+  <depend>geometry_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>generate_parameter_library</depend>
   <depend>rclcpp</depend>
   <depend>rcutils</depend>
   <depend>realtime_tools</depend>

src/control_filters/README.md

@@ -0,0 +1,31 @@
+# Control filters
+
+## Available filters
+
+* Low Pass: implements a low-pass filter based on a time-invariant [Infinite Impulse Response (IIR) filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), for different data types (doubles or wrench).
+
+## Low Pass filter
+
+This filter implements a low-pass filter in the form of an [IIR filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), applied to a `data_in` (double or wrench).
+The feedforward and feedback coefficients of the IIR filter are computed from the low-pass filter parameters.
+
+### Required parameters
+
+* sampling frequency as `sf`
+* damping frequency as `df`
+* damping intensity as `di`
+
+### Algorithm
+
+Given
+* above-required parameters,  `sf`, `df`, `di`
+* `data_in`, a double or wrench `x`
+
+Compute `data_out`, the filtered output `y(n)` with equation:
+
+y(n) = b x(n-1) + a y(n-1)
+
+with
+
+* a the feedbackward coefficient such that a = exp( -1/sf (2 pi df) / (10^(di/-10)) )
+* b the feedforward coefficient such that b = 1 - a