Merge remote-tracking branch 'upstream/master' into ros2-master

Author: ViktorCVS

.pre-commit-config.yaml

@@ -36,7 +36,7 @@ repos:
 
   # Python hooks
   - repo: https://github.com/asottile/pyupgrade
-    rev: v3.20.0
+    rev: v3.21.2
     hooks:
     -   id: pyupgrade
         args: [--py36-plus]
@@ -49,7 +49,7 @@ repos:
       args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
 
   - repo: https://github.com/psf/black
-    rev: 25.9.0
+    rev: 25.12.0
     hooks:
       - id: black
         args: ["--line-length=99"]
@@ -62,7 +62,7 @@ repos:
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v21.1.2
+    rev: v21.1.8
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -136,7 +136,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.34.0
+    rev: 0.36.0
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

control_toolbox/CHANGELOG.rst

@@ -2,6 +2,13 @@
 Changelog for package control_toolbox
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.1.0 (2025-12-01)
+------------------
+* Exponantial filter refactoring (`#493 <https://github.com/ros-controls/control_toolbox/issues/493>`_)
+* Replace deprecated rclcpp::spin_some() (`#541 <https://github.com/ros-controls/control_toolbox/issues/541>`_)
+* No need for the specialization of update() (`#509 <https://github.com/ros-controls/control_toolbox/issues/509>`_)
+* Contributors: Abdullah, Christoph Fröhlich, silanus
+
 6.0.0 (2025-10-16)
 ------------------
 

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_filters/low_pass_filter.hpp

@@ -134,46 +134,6 @@ bool LowPassFilter<T>::configure()
   return lpf_->configure();
 }
 
-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_ || !lpf_ || !lpf_->is_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();
-    lpf_->set_params(
-      parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity);
-  }
-
-  return lpf_->update(data_in, data_out);
-}
-
-template <>
-inline bool LowPassFilter<std::vector<double>>::update(
-  const std::vector<double> & data_in, std::vector<double> & data_out)
-{
-  if (!this->configured_ || !lpf_ || !lpf_->is_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();
-    lpf_->set_params(
-      parameters_.sampling_frequency, parameters_.damping_frequency, parameters_.damping_intensity);
-  }
-
-  return lpf_->update(data_in, data_out);
-}
-
 template <typename T>
 bool LowPassFilter<T>::update(const T & data_in, T & data_out)
 {

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/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>control_toolbox</name>
-  <version>6.0.0</version>
+  <version>6.1.0</version>
   <description>The control toolbox contains modules that are useful across all controllers.</description>
 
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>

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>)