Exponantial filter refactoring

control_toolbox/include/control_filters/exponential_filter.hpp

@@ -18,9 +18,12 @@
 #include <limits>
 #include <memory>
 #include <string>
+#include <vector>
 
 #include "filters/filter_base.hpp"
+#include "geometry_msgs/msg/wrench_stamped.hpp"
 
+#include "control_toolbox/exponential_filter.hpp"
 #include "control_toolbox/exponential_filter_parameters.hpp"
 #include "control_toolbox/filters.hpp"
 
@@ -67,7 +70,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 +103,21 @@ 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)
+template <>
+inline bool ExponentialFilter<geometry_msgs::msg::WrenchStamped>::update(
+  const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out)
 {
-  if (!this->configured_)
+  if (!this->configured_ || !expo_ || !expo_->is_configured())
   {
     throw std::runtime_error("Filter is not configured");
   }
@@ -118,18 +126,51 @@ 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);
+  }
+
+  // Delegate filtering to toolbox filter instance
+  return expo_->update(data_in, data_out);
+}
+
+template <>
+inline bool ExponentialFilter<std::vector<double>>::update(
+  const std::vector<double> & data_in, std::vector<double> & data_out)
+{
+  if (!this->configured_ || !expo_ || !expo_->is_configured())
+  {
+    throw std::runtime_error("Filter is not configured");
   }
 
-  if (std::isnan(last_smoothed_value))
+  // Update internal parameters if required
+  if (parameter_handler_->is_old(parameters_))
   {
-    last_smoothed_value = data_in;
+    parameters_ = parameter_handler_->get_params();
+    expo_->set_params(parameters_.alpha);
   }
 
-  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);
 }
 
+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();
+    expo_->set_params(parameters_.alpha);
+  }
+
+  // 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,117 @@
+// 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 "geometry_msgs/msg/wrench_stamped.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 input_value, output_value, 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(output_value);
+  Traits::initialize(input_value);
+  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(output_value) || Traits::is_empty(output_value))
+  {
+    if (!Traits::is_finite(data_in))
+    {
+      return false;
+    }
+    Traits::assign(old_value, data_in);
+  }
+  else
+  {
+    Traits::validate_input(data_in, output_value, data_out);
+  }
+
+  Traits::assign(input_value, data_in);
+  // Exponential filter update: y[n] = α * x[n] + (1-α) * y[n-1]
+  output_value = alpha_ * input_value + (1.0 - alpha_) * old_value;
+  old_value = output_value;
+
+  Traits::assign(data_out, output_value);
+  Traits::add_metadata(data_out, data_in);
+
+  return true;
+}
+
+}  // namespace control_toolbox
+
+#endif  // CONTROL_TOOLBOX__EXPONENTIAL_FILTER_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>)