Add filter plugin for exponential filter

CMakeLists.txt

@@ -98,6 +98,25 @@ target_link_libraries(rate_limiter PUBLIC
 )
 ament_target_dependencies(rate_limiter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS})
 
+generate_parameter_library(
+  exponential_filter_parameters
+  src/control_filters/exponential_filter_parameters.yaml
+)
+
+add_library(exponential_filter SHARED
+  src/control_filters/exponential_filter.cpp
+)
+target_compile_features(exponential_filter PUBLIC cxx_std_17)
+target_include_directories(exponential_filter PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
+    $<INSTALL_INTERFACE:include/control_toolbox>
+)
+target_link_libraries(exponential_filter PUBLIC
+  exponential_filter_parameters
+)
+ament_target_dependencies(exponential_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS})
+
 # Install pluginlib xml
 pluginlib_export_plugin_description_file(filters control_filters.xml)
 

control_filters.xml

@@ -15,6 +15,15 @@
         </description>
       </class>
     </library>
+    <library path="exponential_filter">
+      <class name="control_filters/ExponentialFilterDouble"
+            type="control_filters::ExponentialFilter&lt;double&gt;"
+            base_class_type="filters::FilterBase&lt;double&gt;">
+        <description>
+            This is an exponential filter working with a double value.
+        </description>
+      </class>
+    </library>
     <library path="rate_limiter">
       <class name="control_filters/RateLimiterDouble"
              type="control_filters::RateLimiter&lt;double&gt;"

include/control_filters/exponential_filter.hpp

@@ -0,0 +1,133 @@
+// Copyright (c) 2024, AIT Austrian Institute of Technology GmbH
+//
+// 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__EXPONENTIAL_FILTER_HPP_
+#define CONTROL_FILTERS__EXPONENTIAL_FILTER_HPP_
+
+#include <memory>
+#include <limits>
+#include <string>
+
+#include "filters/filter_base.hpp"
+
+#include "control_toolbox/filters.hpp"
+#include "exponential_filter_parameters.hpp"
+
+namespace control_filters
+{
+
+/***************************************************/
+/*! \class ExponentialFilter
+  \brief A exponential filter class.
+
+  This class implements a low-pass filter for
+  various data types.
+
+  \section Usage
+
+  The ExponentialFilter 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 ExponentialFilter : public filters::FilterBase<T>
+{
+public:
+  /*!
+   * \brief Configure the ExponentialFilter (access and process params).
+   */
+  bool configure() override;
+
+  /*!
+   * \brief Applies one iteration of the exponential 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<exponential_filter::ParamListener> parameter_handler_;
+  exponential_filter::Params parameters_;
+  T last_smoothed_value;
+};
+
+template <typename T>
+bool ExponentialFilter<T>::configure()
+{
+  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<exponential_filter::ParamListener>(this->params_interface_,
+                                                         this->param_prefix_);
+    }
+    catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+      RCLCPP_ERROR((*logger_), "Exponential filter cannot be configured: %s", ex.what());
+      parameter_handler_.reset();
+      return false;
+    }
+    catch (rclcpp::exceptions::InvalidParameterValueException & ex)  {
+      RCLCPP_ERROR((*logger_), "Exponential filter cannot be configured: %s", ex.what());
+      parameter_handler_.reset();
+      return false;
+    }
+  }
+  parameters_ = parameter_handler_->get_params();
+
+  last_smoothed_value = std::numeric_limits<double>::quiet_NaN();
+
+  return true;
+}
+
+template <typename T>
+bool ExponentialFilter<T>::update(const T & data_in, T & data_out)
+{
+  if (!this->configured_)
+  {
+    throw std::runtime_error("Filter is not configured");
+  }
+
+  // Update internal parameters if required
+  if (parameter_handler_->is_old(parameters_))
+  {
+    parameters_ = parameter_handler_->get_params();
+  }
+
+  if (std::isnan(last_smoothed_value))
+  {
+    last_smoothed_value = data_in;
+  }
+
+  data_out = last_smoothed_value =
+    filters::exponentialSmoothing(data_in, last_smoothed_value, parameters_.alpha);
+  return true;
+}
+
+}  // namespace control_filters
+
+#endif  // CONTROL_FILTERS__EXPONENTIAL_FILTER_HPP_

src/control_filters/README.md

@@ -1,8 +1,11 @@
 # Control filters
 
+Implement filter plugins for control purposes as https://index.ros.org/r/filters/github-ros-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).
+* Exponential Filter: Exponential filter for double data type.
 
 ## Low Pass filter
 
@@ -29,3 +32,13 @@ 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
+
+
+## Exponential filter
+
+### Required parameters
+* `alpha`: the exponential decay factor
+
+### Algorithm
+
+  smoothed_value  = alpha * current_value + (1 - alpha) * last_smoothed_value;

src/control_filters/exponential_filter.cpp

@@ -0,0 +1,19 @@
+// Copyright (c) 2024, AIT Austrian Institute of Technology GmbH
+//
+// 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.
+
+#include "control_filters/exponential_filter.hpp"
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(control_filters::ExponentialFilter<double>, filters::FilterBase<double>)

src/control_filters/exponential_filter_parameters.yaml

@@ -0,0 +1,8 @@
+exponential_filter:
+  alpha: {
+    type: double,
+    description: "Exponential decay factor",
+    validation: {
+      bounds<>: [0.0, 1.0]
+    },
+  }

test/control_filters/test_exponential_filter.cpp

@@ -0,0 +1,65 @@
+// Copyright (c) 2024, AIT Austrian Institute of Technology GmbH
+//
+// 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.
+
+#include "test_exponential_filter.hpp"
+
+TEST_F(ExponentialFilterTest, TestExponentialFilterThrowsUnconfigured)
+{
+    std::shared_ptr<filters::FilterBase<double>> filter_ =
+        std::make_shared<control_filters::ExponentialFilter<double>>();
+    double in, out;
+    ASSERT_THROW(filter_->update(in, out), std::runtime_error);
+}
+
+
+TEST_F(ExponentialFilterTest, TestExponentialFilterComputation)
+{
+    // parameters should match the test yaml file
+    double alpha = 0.7;
+
+    double in = 1.0, calculated, out;
+
+    std::shared_ptr<filters::FilterBase<double>> filter_ =
+        std::make_shared<control_filters::ExponentialFilter<double>>();
+
+    // configure
+    ASSERT_TRUE(filter_->configure("", "TestExponentialFilter",
+        node_->get_node_logging_interface(), node_->get_node_parameters_interface()));
+
+    // first filter pass, output should be input value as no old value was stored
+    ASSERT_TRUE(filter_->update(in, out));
+    ASSERT_EQ(out, 1.0);
+
+    // second filter pass with same values: no change
+      // check equality with low-pass-filter
+      ASSERT_TRUE(filter_->update(in, out));
+      calculated = in;
+      ASSERT_EQ(calculated, out);
+
+    // input change
+      in = 0.0;
+    for (int i = 0; i < 100; ++i){
+      ASSERT_TRUE(filter_->update(in, out));
+      calculated = alpha * in + (1 - alpha) * calculated;
+      ASSERT_EQ(calculated, out);}
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}

test/control_filters/test_exponential_filter.hpp

@@ -0,0 +1,62 @@
+// Copyright (c) 2024, AIT Austrian Institute of Technology GmbH
+//
+// 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__TEST_EXPONENTIAL_FILTER_HPP_
+#define CONTROL_FILTERS__TEST_EXPONENTIAL_FILTER_HPP_
+
+#include <memory>
+#include <thread>
+#include "gmock/gmock.h"
+
+#include "control_filters/exponential_filter.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace
+{
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_exponential_filter");
+}  // namespace
+
+class ExponentialFilterTest : public ::testing::Test
+{
+public:
+  void SetUp() override
+  {
+    auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name();
+    node_ = std::make_shared<rclcpp::Node>(testname);
+    executor_->add_node(node_);
+    executor_thread_ = std::thread([this]() { executor_->spin(); });
+  }
+
+  ExponentialFilterTest()
+  {
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  }
+
+  void TearDown() override
+  {
+    executor_->cancel();
+    if (executor_thread_.joinable())
+    {
+      executor_thread_.join();
+    }
+    node_.reset();
+  }
+
+protected:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Executor::SharedPtr executor_;
+  std::thread executor_thread_;
+};
+
+#endif  // CONTROL_FILTERS__TEST_EXPONENTIAL_FILTER_HPP_

test/control_filters/test_exponential_filter_parameters.yaml

@@ -0,0 +1,3 @@
+TestExponentialFilterComputation:
+  ros__parameters:
+    alpha: 0.7