Branch data Line data Source code
1 : : // Copyright 2020 the Autoware Foundation
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 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 : : /// \file
17 : : /// \brief Interface definition for signal processing filters
18 : : #ifndef SIGNAL_FILTERS__SIGNAL_FILTER_HPP_
19 : : #define SIGNAL_FILTERS__SIGNAL_FILTER_HPP_
20 : :
21 : : #include <common/types.hpp>
22 : : #include <signal_filters/visibility_control.hpp>
23 : :
24 : : #include <algorithm>
25 : : #include <chrono>
26 : : #include <cmath>
27 : : #include <stdexcept>
28 : : #include <type_traits>
29 : :
30 : : using autoware::common::types::bool8_t;
31 : :
32 : : namespace autoware
33 : : {
34 : : namespace common
35 : : {
36 : : namespace signal_filters
37 : : {
38 : :
39 : : /// Fake clock to denote that the duration API is supposed to be used
40 : : struct DummyClock
41 : : {
42 : 4 : static auto now() {return std::chrono::steady_clock::now();} // Just for test convenience
43 : : using time_point = std::chrono::steady_clock::time_point; // Just for test convenience
44 : : };
45 : :
46 : : /// Interface class for filters in the signal processing sense; attenuate response from
47 : : /// various frequency domains
48 : : /// \tparam T A floating point type for the signal
49 : : /// \tparam ClockT The (std::)clock type intended to be used. Setting this implies that the
50 : : /// time_point-based API is going to be used (and that the duration API can't be used)
51 : : template<typename T, typename ClockT = DummyClock>
52 : : class SIGNAL_FILTERS_PUBLIC FilterBase
53 : : {
54 : : static_assert(std::is_floating_point<T>::value, "Filters require a floating point type");
55 : : constexpr static bool8_t use_time_point_api = !std::is_same<ClockT, DummyClock>::value;
56 : :
57 : : public:
58 : : using clock_type = ClockT;
59 : : using signal_type = T;
60 : :
61 : : /// Destructor
62 : : virtual ~FilterBase() = default;
63 : : /// Primary API: receives a value and outputs the result of the filter
64 : : /// \param[in] value An observation on the signal
65 : : /// \param[in] time_stamp The time of the current observation
66 : : /// \return The result of the filter
67 : : /// \throw std::domain_error If time_stamp goes back in time
68 : : /// \throw std::domain_error If value is not normal
69 : : /// \tparam DummyT Dummy type to get SFINAE to work
70 : : template<typename DummyT = T, typename = std::enable_if_t<use_time_point_api, DummyT>>
71 : : T filter(T value, typename clock_type::time_point time_stamp)
72 : : {
73 : 832 : const auto dt = time_stamp - m_last_observation_stamp;
74 [ + - + - : 832 : const auto ret = filter_impl_checked(value, dt);
- + - + -
+ - + - +
- + + - -
+ - + - +
- + - + -
+ - + + -
- + - + -
+ - + - +
- + - + +
- - + - +
- + - + -
+ - + - +
+ - - + ]
75 [ + - + - ]: 804 : m_last_observation_stamp = time_stamp; // Goes after just in case there's an exception
76 : : return ret;
77 : : }
78 : : /// Primary API: receives a value and outputs the result of the filter
79 : : /// \param[in] value An observation on the signal
80 : : /// \param[in] duration Time since last observation, must be positive
81 : : /// \return The result of the filter
82 : : /// \throw std::domain_error If duration is negative
83 : : /// \throw std::domain_error If value is not normal
84 : : /// \tparam DummyT Dummy type to get SFINAE to work
85 : : template<typename DummyT = T, typename = std::enable_if_t<!use_time_point_api, DummyT>>
86 : : T filter(T value, std::chrono::nanoseconds duration)
87 : : {
88 [ + - - + : 416 : return filter_impl_checked(value, duration);
- + - + -
+ - + - +
+ - - + -
+ - + - +
- + - + -
+ + - -
+ ]
89 : : }
90 : :
91 : : protected:
92 : : /// Underlying implementation, with error checking
93 [ + + ]: 1248 : T filter_impl_checked(T value, std::chrono::nanoseconds duration)
94 : : {
95 [ + + ]: 1248 : if (decltype(duration)::zero() >= duration) {
96 [ + - ]: 6 : throw std::domain_error{"Duration is negative"};
97 : : }
98 [ + + ]: 1242 : if (!std::isfinite(value)) {
99 [ + - ]: 36 : throw std::domain_error{"Value is not finite"};
100 : : }
101 : 1206 : return filter_impl(value, duration);
102 : : }
103 : : /// Actual implementation, error checking already done, can assume duration is positive
104 : : virtual T filter_impl(T value, std::chrono::nanoseconds duration) = 0;
105 : :
106 : : private:
107 : : typename clock_type::time_point m_last_observation_stamp{};
108 : : };
109 : :
110 : : } // namespace signal_filters
111 : : } // namespace common
112 : : } // namespace autoware
113 : :
114 : : #endif // SIGNAL_FILTERS__SIGNAL_FILTER_HPP_
|
