Skip to content

Commit 0b0561c

Browse files
christophfroehlichchristophfroehlich
authored
Merge branch 'master' into add/string/informative_methods
2 parents cae03d8 + a2c47db commit 0b0561c

22 files changed

+654
-375
lines changed

‎.pre-commit-config.yaml‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ repos:
3636

3737
# Python hooks
3838
- repo: https://github.com/asottile/pyupgrade
39-
rev: v3.21.0
39+
rev: v3.21.2
4040
hooks:
4141
- id: pyupgrade
4242
args: [--py36-plus]
@@ -49,7 +49,7 @@ repos:
4949
args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
5050

5151
- repo: https://github.com/psf/black
52-
rev: 25.9.0
52+
rev: 26.1.0
5353
hooks:
5454
- id: black
5555
args: ["--line-length=99"]
@@ -62,7 +62,7 @@ repos:
6262

6363
# CPP hooks
6464
- repo: https://github.com/pre-commit/mirrors-clang-format
65-
rev: v21.1.2
65+
rev: v21.1.8
6666
hooks:
6767
- id: clang-format
6868
args: ['-fallback-style=none', '-i']
@@ -136,7 +136,7 @@ repos:
136136
exclude: CHANGELOG\.rst|\.(svg|pyc)$
137137

138138
- repo: https://github.com/python-jsonschema/check-jsonschema
139-
rev: 0.34.1
139+
rev: 0.36.1
140140
hooks:
141141
- id: check-github-workflows
142142
args: ["--verbose"]

‎control_toolbox/CHANGELOG.rst‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,13 @@
22
Changelog for package control_toolbox
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
6.1.0 (2025-12-01)
6+
------------------
7+
* Exponantial filter refactoring (`#493 <https://github.com/ros-controls/control_toolbox/issues/493>`_)
8+
* Replace deprecated rclcpp::spin_some() (`#541 <https://github.com/ros-controls/control_toolbox/issues/541>`_)
9+
* No need for the specialization of update() (`#509 <https://github.com/ros-controls/control_toolbox/issues/509>`_)
10+
* Contributors: Abdullah, Christoph Fröhlich, silanus
11+
512
6.0.0 (2025-10-16)
613
------------------
714

‎control_toolbox/include/control_filters/exponential_filter.hpp‎

Lines changed: 14 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -18,11 +18,12 @@
1818
#include <limits>
1919
#include <memory>
2020
#include <string>
21+
#include <vector>
2122

2223
#include "filters/filter_base.hpp"
2324

25+
#include "control_toolbox/exponential_filter.hpp"
2426
#include "control_toolbox/exponential_filter_parameters.hpp"
25-
#include "control_toolbox/filters.hpp"
2627

2728
namespace control_filters
2829
{
@@ -67,7 +68,7 @@ class ExponentialFilter : public filters::FilterBase<T>
6768
std::shared_ptr<rclcpp::Logger> logger_;
6869
std::shared_ptr<exponential_filter::ParamListener> parameter_handler_;
6970
exponential_filter::Params parameters_;
70-
T last_smoothed_value;
71+
std::shared_ptr<control_toolbox::ExponentialFilter<T>> expo_;
7172
};
7273

7374
template <typename T>
@@ -100,16 +101,20 @@ bool ExponentialFilter<T>::configure()
100101
}
101102
}
102103
parameters_ = parameter_handler_->get_params();
104+
expo_ = std::make_shared<control_toolbox::ExponentialFilter<T>>(parameters_.alpha);
103105

104-
last_smoothed_value = std::numeric_limits<double>::quiet_NaN();
105-
106-
return true;
106+
bool configured = expo_->configure();
107+
if (!configured)
108+
{
109+
RCLCPP_ERROR((*logger_), "ExponentialFilter: Failed to configure underlying filter instance.");
110+
}
111+
return configured;
107112
}
108113

109114
template <typename T>
110115
bool ExponentialFilter<T>::update(const T & data_in, T & data_out)
111116
{
112-
if (!this->configured_)
117+
if (!this->configured_ || !expo_ || !expo_->is_configured())
113118
{
114119
throw std::runtime_error("Filter is not configured");
115120
}
@@ -118,18 +123,12 @@ bool ExponentialFilter<T>::update(const T & data_in, T & data_out)
118123
if (parameter_handler_->is_old(parameters_))
119124
{
120125
parameters_ = parameter_handler_->get_params();
126+
expo_->set_params(parameters_.alpha);
121127
}
122128

123-
if (std::isnan(last_smoothed_value))
124-
{
125-
last_smoothed_value = data_in;
126-
}
127-
128-
data_out = last_smoothed_value =
129-
filters::exponentialSmoothing(data_in, last_smoothed_value, parameters_.alpha);
130-
return true;
129+
// Delegate filtering to toolbox filter instance
130+
return expo_->update(data_in, data_out);
131131
}
132-
133132
} // namespace control_filters
134133

135134
#endif // CONTROL_FILTERS__EXPONENTIAL_FILTER_HPP_

‎control_toolbox/include/control_toolbox/dither.hpp‎

Lines changed: 21 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -1,33 +1,29 @@
11
// Copyright (c) 2009, Willow Garage, Inc.
2-
// All rights reserved.
3-
//
4-
// Software License Agreement (BSD License 2.0)
52
//
63
// Redistribution and use in source and binary forms, with or without
7-
// modification, are permitted provided that the following conditions
8-
// are met:
4+
// modification, are permitted provided that the following conditions are met:
5+
//
6+
// * Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
//
9+
// * Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
912
//
10-
// * Redistributions of source code must retain the above copyright
11-
// notice, this list of conditions and the following disclaimer.
12-
// * Redistributions in binary form must reproduce the above
13-
// copyright notice, this list of conditions and the following
14-
// disclaimer in the documentation and/or other materials provided
15-
// with the distribution.
16-
// * Neither the name of the Willow Garage nor the names of its
17-
// contributors may be used to endorse or promote products derived
18-
// from this software without specific prior written permission.
13+
// * Neither the name of the Willow Garage nor the names of its
14+
// contributors may be used to endorse or promote products derived from
15+
// this software without specific prior written permission.
1916
//
20-
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21-
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22-
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23-
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24-
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25-
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26-
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27-
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28-
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29-
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30-
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
17+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
3127
// POSSIBILITY OF SUCH DAMAGE.
3228

3329
// \author Kevin Watts

‎control_toolbox/include/control_toolbox/exponential_filter.hpp‎

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef CONTROL_TOOLBOX__EXPONENTIAL_FILTER_HPP_
16+
#define CONTROL_TOOLBOX__EXPONENTIAL_FILTER_HPP_
17+
18+
#include <cmath>
19+
#include <limits>
20+
#include <memory>
21+
#include <stdexcept>
22+
#include <string>
23+
#include <type_traits>
24+
#include <vector>
25+
26+
#include "control_toolbox/filter_traits.hpp"
27+
#include "control_toolbox/filters.hpp"
28+
29+
namespace control_toolbox
30+
{
31+
template <typename T>
32+
class ExponentialFilter
33+
{
34+
public:
35+
// Default constructor
36+
ExponentialFilter();
37+
explicit ExponentialFilter(double alpha) { set_params(alpha); }
38+
39+
~ExponentialFilter();
40+
41+
bool configure();
42+
43+
bool update(const T & data_in, T & data_out);
44+
45+
bool is_configured() const { return configured_; }
46+
47+
void set_params(double alpha) { alpha_ = alpha; }
48+
49+
private:
50+
double alpha_;
51+
52+
// Define the storage type based on T
53+
using Traits = FilterTraits<T>;
54+
using StorageType = typename Traits::StorageType;
55+
56+
StorageType old_value_;
57+
58+
bool configured_ = false;
59+
};
60+
61+
template <typename T>
62+
ExponentialFilter<T>::ExponentialFilter() : alpha_(0.5)
63+
{
64+
}
65+
66+
template <typename T>
67+
ExponentialFilter<T>::~ExponentialFilter()
68+
{
69+
}
70+
71+
template <typename T>
72+
bool ExponentialFilter<T>::configure()
73+
{
74+
Traits::initialize(old_value_);
75+
76+
return configured_ = true;
77+
}
78+
79+
template <typename T>
80+
bool ExponentialFilter<T>::update(const T & data_in, T & data_out)
81+
{
82+
if (!configured_)
83+
{
84+
throw std::runtime_error("Filter is not configured");
85+
}
86+
87+
// First call: initialize filter state
88+
if (Traits::is_nan(old_value_) || Traits::is_empty(old_value_))
89+
{
90+
if (!Traits::is_finite(data_in))
91+
{
92+
return false;
93+
}
94+
Traits::assign(old_value_, data_in);
95+
}
96+
else
97+
{
98+
Traits::validate_input(data_in, old_value_, data_out);
99+
}
100+
101+
// Convert data_in to StorageType for arithmetic
102+
StorageType storage_in;
103+
Traits::assign(storage_in, data_in);
104+
105+
// Exponential filter update using the templated exponentialSmoothing function
106+
old_value_ = filters::exponentialSmoothing(storage_in, old_value_, alpha_);
107+
108+
Traits::assign(data_out, old_value_);
109+
Traits::add_metadata(data_out, data_in);
110+
111+
return true;
112+
}
113+
114+
} // namespace control_toolbox
115+
116+
#endif // CONTROL_TOOLBOX__EXPONENTIAL_FILTER_HPP_

‎control_toolbox/include/control_toolbox/filters.hpp‎

Lines changed: 25 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -1,33 +1,29 @@
11
// Copyright (c) 2008, Willow Garage, Inc.
2-
// All rights reserved.
3-
//
4-
// Software License Agreement (BSD License 2.0)
52
//
63
// Redistribution and use in source and binary forms, with or without
7-
// modification, are permitted provided that the following conditions
8-
// are met:
4+
// modification, are permitted provided that the following conditions are met:
5+
//
6+
// * Redistributions of source code must retain the above copyright
7+
// notice, this list of conditions and the following disclaimer.
8+
//
9+
// * Redistributions in binary form must reproduce the above copyright
10+
// notice, this list of conditions and the following disclaimer in the
11+
// documentation and/or other materials provided with the distribution.
912
//
10-
// * Redistributions of source code must retain the above copyright
11-
// notice, this list of conditions and the following disclaimer.
12-
// * Redistributions in binary form must reproduce the above
13-
// copyright notice, this list of conditions and the following
14-
// disclaimer in the documentation and/or other materials provided
15-
// with the distribution.
16-
// * Neither the name of the Willow Garage nor the names of its
17-
// contributors may be used to endorse or promote products derived
18-
// from this software without specific prior written permission.
13+
// * Neither the name of the Willow Garage nor the names of its
14+
// contributors may be used to endorse or promote products derived from
15+
// this software without specific prior written permission.
1916
//
20-
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21-
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22-
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23-
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24-
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25-
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26-
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27-
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28-
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29-
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30-
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
17+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
3127
// POSSIBILITY OF SUCH DAMAGE.
3228

3329
#ifndef CONTROL_TOOLBOX__FILTERS_HPP_
@@ -40,11 +36,12 @@ namespace filters
4036
/** Exponential smoothing filter. Alpha is between 0 and 1.
4137
* Values closer to 0 weight the last smoothed value more heavily */
4238

43-
static inline double exponentialSmoothing(
44-
double current_raw_value, double last_smoothed_value, double alpha)
39+
template <typename T>
40+
static inline T exponentialSmoothing(
41+
const T & current_raw_value, const T & last_smoothed_value, double alpha)
4542
{
4643
return alpha * current_raw_value + (1 - alpha) * last_smoothed_value;
4744
}
4845
} // namespace filters
4946

50-
#endif // CONTROL_TOOLBOX__FILTERS_HPP_"
47+
#endif // CONTROL_TOOLBOX__FILTERS_HPP_

0 commit comments

Comments
 (0)