Autoware.Auto
signal_filter.hpp
Go to the documentation of this file.
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.
18 #ifndef SIGNAL_FILTERS__SIGNAL_FILTER_HPP_
19 #define SIGNAL_FILTERS__SIGNAL_FILTER_HPP_
20 
21 #include <common/types.hpp>
23 
24 #include <algorithm>
25 #include <chrono>
26 #include <cmath>
27 #include <stdexcept>
28 #include <type_traits>
29 
31 
32 namespace autoware
33 {
34 namespace common
35 {
36 namespace signal_filters
37 {
38 
40 struct DummyClock
41 {
42  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 
51 template<typename T, typename ClockT = DummyClock>
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 
62  virtual ~FilterBase() = default;
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  const auto dt = time_stamp - m_last_observation_stamp;
74  const auto ret = filter_impl_checked(value, dt);
75  m_last_observation_stamp = time_stamp; // Goes after just in case there's an exception
76  return ret;
77  }
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  return filter_impl_checked(value, duration);
89  }
90 
91 protected:
93  T filter_impl_checked(T value, std::chrono::nanoseconds duration)
94  {
95  if (decltype(duration)::zero() >= duration) {
96  throw std::domain_error{"Duration is negative"};
97  }
98  if (!std::isfinite(value)) {
99  throw std::domain_error{"Value is not finite"};
100  }
101  return filter_impl(value, duration);
102  }
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_
autoware::common::signal_filters::FilterBase::filter
T filter(T value, std::chrono::nanoseconds duration)
Definition: signal_filter.hpp:86
types.hpp
This file includes common type definition.
autoware::common::signal_filters::FilterBase::filter
T filter(T value, typename clock_type::time_point time_stamp)
Definition: signal_filter.hpp:71
autoware::common::signal_filters::DummyClock
Fake clock to denote that the duration API is supposed to be used.
Definition: signal_filter.hpp:40
autoware::common::signal_filters::FilterBase< T, std::chrono::steady_clock >::clock_type
std::chrono::steady_clock clock_type
Definition: signal_filter.hpp:58
visibility_control.hpp
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
SIGNAL_FILTERS_PUBLIC
#define SIGNAL_FILTERS_PUBLIC
Definition: common/signal_filters/include/signal_filters/visibility_control.hpp:44
autoware::common::signal_filters::FilterBase
Definition: signal_filter.hpp:52
autoware::common::signal_filters::FilterBase< T, std::chrono::steady_clock >::signal_type
T signal_type
Definition: signal_filter.hpp:59
autoware::common::signal_filters::DummyClock::now
static auto now()
Definition: signal_filter.hpp:42
autoware::common::signal_filters::FilterBase::filter_impl_checked
T filter_impl_checked(T value, std::chrono::nanoseconds duration)
Underlying implementation, with error checking.
Definition: signal_filter.hpp:93
autoware::common::signal_filters::DummyClock::time_point
std::chrono::steady_clock::time_point time_point
Definition: signal_filter.hpp:43