Exponantial filter refactoring (#493)

Author: silanus23

control_toolbox/include/control_filters/exponential_filter.hpp

@@ -18,11 +18,12 @@
 #include <limits>
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "filters/filter_base.hpp"
 
+#include "control_toolbox/exponential_filter.hpp"
 #include "control_toolbox/exponential_filter_parameters.hpp"
-#include "control_toolbox/filters.hpp"
 
 namespace control_filters
 {
@@ -67,7 +68,7 @@ class ExponentialFilter : public filters::FilterBase<T>
   std::shared_ptr<rclcpp::Logger> logger_;
   std::shared_ptr<exponential_filter::ParamListener> parameter_handler_;
   exponential_filter::Params parameters_;
-  T last_smoothed_value;
+  std::shared_ptr<control_toolbox::ExponentialFilter<T>> expo_;
 };
 
 template <typename T>
@@ -100,16 +101,20 @@ bool ExponentialFilter<T>::configure()
     }
   }
   parameters_ = parameter_handler_->get_params();
+  expo_ = std::make_shared<control_toolbox::ExponentialFilter<T>>(parameters_.alpha);
 
-  last_smoothed_value = std::numeric_limits<double>::quiet_NaN();
-
-  return true;
+  bool configured = expo_->configure();
+  if (!configured)
+  {
+    RCLCPP_ERROR((*logger_), "ExponentialFilter: Failed to configure underlying filter instance.");
+  }
+  return configured;
 }
 
 template <typename T>
 bool ExponentialFilter<T>::update(const T & data_in, T & data_out)
 {
-  if (!this->configured_)
+  if (!this->configured_ || !expo_ || !expo_->is_configured())
   {
     throw std::runtime_error("Filter is not configured");
   }
@@ -118,18 +123,12 @@ bool ExponentialFilter<T>::update(const T & data_in, T & data_out)
   if (parameter_handler_->is_old(parameters_))
   {
     parameters_ = parameter_handler_->get_params();
+    expo_->set_params(parameters_.alpha);
   }
 
-  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;
+  // Delegate filtering to toolbox filter instance
+  return expo_->update(data_in, data_out);
 }
-
 }  // namespace control_filters
 
 #endif  // CONTROL_FILTERS__EXPONENTIAL_FILTER_HPP_

control_toolbox/include/control_toolbox/exponential_filter.hpp

@@ -0,0 +1,116 @@
+// 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_TOOLBOX__EXPONENTIAL_FILTER_HPP_
+#define CONTROL_TOOLBOX__EXPONENTIAL_FILTER_HPP_
+
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <type_traits>
+#include <vector>
+
+#include "control_toolbox/filter_traits.hpp"
+#include "control_toolbox/filters.hpp"
+
+namespace control_toolbox
+{
+template <typename T>
+class ExponentialFilter
+{
+public:
+  // Default constructor
+  ExponentialFilter();
+  explicit ExponentialFilter(double alpha) { set_params(alpha); }
+
+  ~ExponentialFilter();
+
+  bool configure();
+
+  bool update(const T & data_in, T & data_out);
+
+  bool is_configured() const { return configured_; }
+
+  void set_params(double alpha) { alpha_ = alpha; }
+
+private:
+  double alpha_;
+
+  // Define the storage type based on T
+  using Traits = FilterTraits<T>;
+  using StorageType = typename Traits::StorageType;
+
+  StorageType old_value_;
+
+  bool configured_ = false;
+};
+
+template <typename T>
+ExponentialFilter<T>::ExponentialFilter() : alpha_(0.5)
+{
+}
+
+template <typename T>
+ExponentialFilter<T>::~ExponentialFilter()
+{
+}
+
+template <typename T>
+bool ExponentialFilter<T>::configure()
+{
+  Traits::initialize(old_value_);
+
+  return configured_ = true;
+}
+
+template <typename T>
+bool ExponentialFilter<T>::update(const T & data_in, T & data_out)
+{
+  if (!configured_)
+  {
+    throw std::runtime_error("Filter is not configured");
+  }
+
+  // First call: initialize filter state
+  if (Traits::is_nan(old_value_) || Traits::is_empty(old_value_))
+  {
+    if (!Traits::is_finite(data_in))
+    {
+      return false;
+    }
+    Traits::assign(old_value_, data_in);
+  }
+  else
+  {
+    Traits::validate_input(data_in, old_value_, data_out);
+  }
+
+  // Convert data_in to StorageType for arithmetic
+  StorageType storage_in;
+  Traits::assign(storage_in, data_in);
+
+  // Exponential filter update using the templated exponentialSmoothing function
+  old_value_ = filters::exponentialSmoothing(storage_in, old_value_, alpha_);
+
+  Traits::assign(data_out, old_value_);
+  Traits::add_metadata(data_out, data_in);
+
+  return true;
+}
+
+}  // namespace control_toolbox
+
+#endif  // CONTROL_TOOLBOX__EXPONENTIAL_FILTER_HPP_

control_toolbox/include/control_toolbox/filters.hpp

@@ -40,11 +40,12 @@ namespace filters
 /** Exponential smoothing filter. Alpha is between 0 and 1.
  * Values closer to 0 weight the last smoothed value more heavily */
 
-static inline double exponentialSmoothing(
-  double current_raw_value, double last_smoothed_value, double alpha)
+template <typename T>
+static inline T exponentialSmoothing(
+  const T & current_raw_value, const T & last_smoothed_value, double alpha)
 {
   return alpha * current_raw_value + (1 - alpha) * last_smoothed_value;
 }
 }  // namespace filters
 
-#endif  // CONTROL_TOOLBOX__FILTERS_HPP_"
+#endif  // CONTROL_TOOLBOX__FILTERS_HPP_

control_toolbox/src/control_filters/exponential_filter.cpp

@@ -17,3 +17,10 @@
 #include "pluginlib/class_list_macros.hpp"
 
 PLUGINLIB_EXPORT_CLASS(control_filters::ExponentialFilter<double>, filters::FilterBase<double>)
+
+PLUGINLIB_EXPORT_CLASS(
+  control_filters::ExponentialFilter<std::vector<double>>, filters::FilterBase<std::vector<double>>)
+
+PLUGINLIB_EXPORT_CLASS(
+  control_filters::ExponentialFilter<geometry_msgs::msg::WrenchStamped>,
+  filters::FilterBase<geometry_msgs::msg::WrenchStamped>)